JP2009109200A - Position attitude estimation system, position attitude estimation device, and position attitude estimation method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a position attitude estimation system 10 reducing furthermore a calculation amount, in a matching method based on a sensing data list. <P>SOLUTION: In this position attitude estimation system 10, a means 150 for generating a reference data group performs: a processing 102 for generating the first reference data group 103 from an environmental map 101: and a processing 104 for generating the second reference data group 105 by a differential processing of the first reference data group 103. A means 170 for estimating a position attitude, performs: a processing 120 for selecting necessary data from the first reference data group 103 and the second reference data group 105 based on sensing data 107; a processing 121 for generating data 122 for new evaluation by adding a plurality of selected reference data to data 122 for evaluation generated in the last time; and a processing 123 for determining a position attitude estimation value 124 from the data 122 for evaluation. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a technique for recognizing a position and orientation of a self in an actual environment, which is used in autonomous movement control of a mobile robot. In particular, for the purpose of the mobile robot reaching the destination automatically without human operation, the position and orientation of the mobile robot are shown by sensing data from the distance sensor mounted on the mobile robot and the geometric state of the environment. The present invention relates to a technique for calculating using a two-dimensional environmental map.

  First, a conventional method for estimating the position and orientation of a mobile robot using a distance sensor and a two-dimensional environment map will be described.

Here, the distance sensor can measure the distance to an obstacle in a plurality of directions around the distance sensor. Various specifications can be considered, but here, as shown in FIGS. 13 and 14, 1 ° in a direction from −90 ° to + 90 ° with respect to the front of the distance sensor 421 on a horizontal plane at a certain height. Assume that distance data to the obstacle 422 can be instantaneously measured for a total of 181 different directions for each. Therefore, each sensing data 431 is expressed as (d ₀ , φ ₀ ), (d ₁ , φ ₁ ),..., (D ₁₈₀ , φ ₁₈₀ ) as a combination of the angle φ and the distance d.

Here, the numerical subscript of each variable is a data number, and φ _i indicates the i-th measurement direction. The measurement direction is an angle made counterclockwise as the positive direction and the main direction of the distance sensor, and data numbers are assigned in ascending order of the angle made with the main direction. That is, φ ₀ = −90 °, φ ₁ = −89 °,..., Φ ₁₈₀ = + 90.0 °. D _i indicates the distance to the obstacle in each direction.

Here, considering the ability to measure the distance sensor, the measurable distance assumes greater 4.0m follows from 0.0 m, the range of values of d _i, and be limited to 0.0 m <d _i ≦ 4.0m . Further, the 181 distance data are grouped as one group and referred to as a sensing data group 430 for one frame. The parameter expressed by (d _i , φ _i ) is equivalent to the position of the sensing data, that is, the position of the obstacle expressed in polar coordinates with the origin of the distance sensor as the origin. By setting x _i = d _i · cos (φ _i ) and y _i = d _i · sin (φ _i ), it is also possible to convert to a rectangular coordinate representation of the form (x _i , y _i ).

  On the other hand, the two-dimensional environment map represents the presence of the obstacle 422 on the cross section measured by the distance sensor 421 as a digital image as shown in FIG. Here, as shown in FIG. 15, a place where no obstacle exists is represented as a pixel value 441 indicating white, and a place where an obstacle exists is represented as a pixel value 442 indicating black.

  It is assumed that the mobile robot 420 moves on a flat ground as shown in FIG. Therefore, the degree of freedom relating to the position and orientation that changes due to the movement is a total of three parameters, that is, two parameters (x, y) for the position and one parameter θ for the orientation angle that the mobile robot faces. Note that the coordinate system representing these parameters is made to coincide with the coordinate system representing the environment map as shown in FIG.

  Estimating the position and orientation of the mobile robot means obtaining the three parameters (x, y, θ) from the sensing data of the mobile robot in an arbitrary position and orientation. Therefore, the functions targeted by the present invention are the position and orientation parameters (x, y, θ) of a mobile robot in an arbitrary position and orientation, sensing data from a distance sensor mounted on the mobile robot, and an environment prepared in advance. It will be obtained using a map.

  Several methods can be considered as means for realizing this function. Here, a matching method which is the most basic method will be described.

  The matching method is to superimpose the sensing data on the environment map while changing the position and orientation (direction) of the entire sensing data measured in a certain frame, and find the position and orientation where the sensing data and the environment map best match. This is the estimated value of the position and orientation parameter of the mobile robot. The principle is based on the idea that when the mobile robot is in its position and posture, the same data as the measured data will be obtained with the highest probability.

Such a matching method is specifically implemented by the processing described below.
(1) For all the sensing data in a certain frame (181 in the example of the distance sensor described above), this is rotated by θ on the map, and the position translated by (x, y) is obtained.
(2) Referring to the pixel values on the environment map at those positions, count the number of objects with a value of 1 (there is an obstacle), and use this as the evaluation value for that parameter (x, y, θ). To do.
(3) For the combinations of (x, y, θ) that have all possible values, the solution of (1) and (2) above is the solution with the highest evaluation value (x, y, θ) .

  The above method will be referred to as a matching method based on the basic method. FIG. 16 shows an outline of the matching method based on the basic method. When the environment map 440 shown in FIG. 15 and the sensing data group 430 shown in FIG. 16 (a) are given, the situation determined to be the best match by this method is the position 450 and FIG. 16 (b). The direction is 451. The translation component (x, y) and the rotation component θ at this time are estimated values of the mobile robot position and orientation parameters.

  The matching method based on the basic method has a problem that the processing amount is enormous. Specifically, the size of the environment map, that is, the size of the area where the mobile robot can move, such as the target room, is 10 m × 10 m, for example, and the matching granularity, that is, how fine the parameters are changed Assuming that the minimum unit is 10 cm for the position, ie, (x, y) parameter, and 1 °, for example, the orientation, ie, the θ parameter, the matching method based on the basic method described above is 181 × 360 × 101 × 101. = 664,697,160 pixel value reference operations are required.

  Here, “181” is the number of data, “360” is the number of values that the rotation angle θ can take (360 degrees of 0 °, 1 °, 2 °,..., 359 °), and “101” is the x direction. The number of values that the movement amount of can take (101 of -5.0m, -4.9m, ..., + 5.0m, the next "101" is the number of values that the movement amount in the y direction can take ( -5.0m, -4.9m, ..., + 5.0m 101 ways).

  It is assumed that the matching method is generally performed by a computer or the like. Each of the pixel value reference calculations performed by the matching method based on the basic method described above is not so much computational load, but if the amount is enormous, the computational load increases in proportion to the amount. It will be. The above-described specific example of the number of reference computations generally has an order calculation amount that cannot be ignored in practical use or implementation in consideration of the ability of currently distributed computers.

  As a method for solving such a problem of the matching method based on the basic method that the processing amount becomes enormous, for example, Non-Patent Document 1 proposes a matching method based on a sensing data list sensing data list. Processing based on this idea will be described. Hereinafter, this processing is referred to as a matching method based on the sensing data list.

  In order to simplify the explanation here, it is assumed that the movement of the mobile robot is only a translational motion and no rotational motion is performed. That is, two parameters to be estimated are (x, y), and θ is fixed to 0 here.

  First, consider the matching method based on the basic method described above. Here, it is assumed that a sensing data group 460 and an environment map 463 as shown in FIG. 17 are given. In the drawings used hereinafter, in order to simplify the description, the detail level of pixels is rough and the number of sensing data is reduced. Here, in FIG. 17, reference numeral 461 denotes a sensor origin, 462 denotes sensing data, and the 0th sensing data, the first sensing data, in the order of the numbers (0) to (3) shown in FIG. It will be referred to as second sensing data and third sensing data.

  In the matching method based on the basic method, as shown in FIG. 18, the entire sensing data is overlapped on the environment map while being translated, and the number of pixels where each sensing data points, that is, the pixel value where the sensing data overlaps is 1, It will be counted as an evaluation value. For example, the evaluation value is 0 at (x, y) = (− 3, −3) in FIG. 18, and the evaluation value is 1 at (x, y) = (− 1, −3). When (x, y) = (− 1, −1), the evaluation value is 4, which is the maximum evaluation value. In this case, the estimated value is (x, y) = (− 1, −1).

  Next, a matching method based on the sensing data list will be described.

  Now, paying attention to the 0th sensing data in the sensing data group 460 of FIG. 17, the pixel value on the environment map pointed to by the 0th sensing data is 1 where the entire sensing data group is moved Think about what happened. It can be seen from FIG. 18 that there are 10 such movements as shown in FIG. In addition, 470 shown in FIG. 19 is 0th sensing data. In this example, since there are 10 points with a pixel value of 1 on the environment map, there are 10 movement methods.

  Next, when the origin position 471 of the sensing data at this time is plotted on a blank map, the result is as shown in FIG. The pattern created in this way is referred to as a reference map, more specifically, a reference map 480 relating to the 0th sensing data.

  By the way, if the reference map 480 relating to the 0th sensing data is looked closely, the colored pixel pattern is the pixel component of the environmental map shown in FIG. 17B and the vector component 495 shown in FIG. It can be seen that the translation is only performed. It can also be seen that this vector component 495 is in the opposite direction of the vector determined from the 0th sensing data 494 and the sensor origin 493 in the sensing data group.

Therefore, if the 0th sensing data is (d ₀ , φ ₀ ), the calculation of x ₀ = d ₀ · cos (φ ₀ ), y ₀ = d ₀ · sin (φ ₀ ) The vector component can be expressed by an orthogonal coordinate system of the form (x ₀ , y ₀ ). After calculating this, if the original environmental map is translated in the opposite direction by this vector, the reference map 480 relating to the 0th sensing data is created.

  In the reference map 480 created in this way (the reference map related to the 0th data), the meaning of the pixel on which the sensor origin 481 is plotted is as follows. That is, “if the sensing data is translated so that the origin position is at this position, the 0th sensing data matches the pixel having a value of 1 in the environmental map data”.

  This is the case when the 0th sensing data is considered, but this can be similarly considered for other sensing data. That is, with respect to the nth sensing data, a vector formed by the sensor origin can be considered, and a reference map for the nth sensing data can be created by translating the environment map in the opposite direction by the vector component. it can.

Specifically, when the n-th sensing data is obtained as (d _n , φ _n ) as in the case of the 0-th sensing data, first, the value (x _n , y _n ) of this Cartesian coordinate expression is used. ) Is obtained by calculating x _n = d _n · cos (φ _n ) and y _n = d _n · sin (φ _n ). Next, the map image is translated by this value (x _n , y _n ). The reference map for the n-th sensing data created in this way is obtained by translating the origin of the sensing data to the hatched pixels in the reference map. It means that it matches the pixel having the value of.

  Since there are four sensing data in the above example, as shown in FIG. 22, there are a reference map 500 relating to the 0th sensing data, a reference map 501 relating to the first sensing data, and a second as shown in FIG. A total of four reference maps 502 related to the sensing data of the reference data 503 and reference map 503 related to the third sensing data are prepared.

  Next, as shown in FIG. 23, it is considered that these four reference maps are overlapped and the number of colored pixels overlapped for each pixel is added and counted. Since the meaning of each reference map is as described above, this number has a value of 1 in the environmental map among the four sensing data when the origin of the sensing data moves to the position of each pixel. It represents the number of objects that match the pixel.

  Image data in which the added value is a pixel value is as shown in FIG. 24, for example, and is referred to as an evaluation image 510. That is, the pixel value at the position (x, y) in the evaluation image 510 represents the evaluation value calculated when the sensor is moved by (x, y) in the matching method based on the basic method described above. become. Therefore, similarly to the matching method based on the basic method, if the image having the largest pixel value is found in the evaluation image 510 and the coordinate position of the point on the image is examined, the position of the mobile robot to be obtained is estimated. Value.

Here, the process of superimposing each reference map does not check the pixel value of each image, but the position of a pixel having a value of 1 in each reference map in advance (x ₀ , y ₀ ) , (x ₁ , y ₁ ), ..., (x _n , y _n ) are recorded, and the pixel values of the evaluation image indicated by the recorded coordinate values are incremented one by one. That is, it is performed by a method of voting for the pixel. This has the effect of reducing the number of references to each pixel of the reference map, which will be described later, that is, it is not necessary to refer to a pixel having a value of 0 in the reference map.

Based on this principle, the matching method based on the sensing data list is processed in the following procedure.
(1) Create reference maps for all possible sensing data locations. That is, as described in the above description, a reference map is created for each vector formed by all possible sensing data positions and the sensor origin. Note that the reference map is not actually recorded in the form of image data, but as described above, the positions of the pixels having a value of 1 are sequentially (x ₀ , y ₀ ), (x ₁ , y ₁ ), ..., (x _n , y _n ) shall be recorded.
(2) When a group of sensing data for one frame is given, the position of each sensing data (total 181) and the vector made by the sensor origin are calculated, and the reference map for that vector is selected (total) 181).
(3) Superimpose all the selected reference maps, and create an image that adds pixel values for each pixel as an evaluation image. This will determine the number of colored ones for each pixel.
(4) Find the position (x, y) of the pixel having the maximum pixel value in the created evaluation image, and use (x, y) at this time as the matching solution.

Here, the part of all the possible sensing data described in the above description will be supplemented. As described at the beginning, sensing data by the distance sensor is expressed in the form of (d ₀ , φ ₀ ), (d ₁ , φ ₁ ),..., (D ₁₈₀ , φ ₁₈₀ ). Here, with respect to the angle φ _i , the sensing direction is fixed by 1 °, so that φ ₀ = −90 °, φ ₁ = −89 °,..., Φ ₁₈₀ = + 90 ° become.

However, since the distance d _i can take various values within the measurement range (0.0 m <d _i ≦ 4.0 m), this can be considered infinitely. Therefore, it is considered that this is represented by a predetermined discrete value. In other words, for example, 40 values of 0.1 m, 0.2 m,..., 4.0 m are considered as representative values, and if d _i ≦ 0.15 m, then d _i = 0.1 m and 0.15 m <d _i ≦ 0.25 m. , Assign values with rules such as d _i = 0.2m. In this way, the sensing data (d _{i, φ i)} combinations of possible values of the 40 types for d _i, it means that there 180 types for phi _i, all the sensing data may think, Total 40 × 180 finite pieces.

  This is nothing but the operation of quantization in so-called digital processing, but such deterioration in accuracy due to quantization performed in units of about 10 cm is used in applications such as initial value calculation in position and orientation estimation, for example. There is almost no problem. Similar to the quantization of the sensing data, the reference map is also quantized in units of 10 cm in the following description. That is, one pixel on the image indicates a 10 cm × 10 cm area in the real environment.

  In the above explanation, it is assumed that the mobile robot moves only in parallel, and the matching is performed with only two parameters (x, y). It is also necessary to obtain a parameter related to (attitude) θ. In this regard, all possible rotations θ, that is, the quantization concept described above is also introduced and 360 values of θ = 0 °, 1 °, 2 °,..., 359 ° as discrete values are introduced. For each of these angles, the entire sensing data group is rotated in advance, and the above-described evaluation images are created based on this (total of 360 images). The pixel having the largest value is found, and the solution (x, y, θ) obtained from the θ value to which the image belongs and the position (x, y) of the pixel is obtained.

  In the above method, reference calculation of pixel values of 181 × 360 × 101 × 101 × 1/2 = about 500 million times or less is performed. Here, the meanings of the numbers “181”, “360”, “101”, and “101” are the same as in the matching method based on the basic method. Here, “1/2” newly added is an approximate ratio of the number of pixels with obstacles to the total number of pixels in the environment map image. In other words, the matching method based on the sensing data list reduces the number of reference computations by this number, in other words, the proportion of pixels in the environmental map image where there are no obstacles, compared to the matching method based on the basic method. become. This is the effect of the sensing data list.

However, on the other hand, a reference map is prepared in advance, and this is used as position information of pixels having a value of 1 as (x ₀ , y ₀ ), (x ₁ , y ₁ ), ..., (x _n , Since it must be recorded as y _n ), two new problems arise: (1) processing for creating a reference map is required, and (2) the memory usage is enormous. Regarding (1), since it is sufficient to create a reference map in advance before the mobile robot is driven, it is considered that there are few practical problems. As for (2), an approximate estimate. In the above example, the required amount of memory is expected to be several gigabytes, which is an order that can be realized based on the performance of the current main memory or auxiliary memory.

L. M. Paz, P. Pinies, J. Neira, J. D. Tardos, "Global Localization in SLAM in Bilinear Time," In Proc. International Conference on Intelligent Robots and Systems, 2005, pp. 1445-1450.

  Regarding the method for estimating the position and orientation of the mobile robot using the sensing data measured by the distance sensor and the environment map, the matching method based on the basic method described above has a problem that the processing amount becomes enormous and the calculation time becomes long. Further, in the matching method based on the sensing data list, although the processing amount is reduced to some extent, it is desired to further reduce the calculation processing from the viewpoint of reducing the weight and cost of the mobile robot.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to realize a further reduction in calculation amount in a matching method based on a sensing data list.

  In order to solve the above-described problems, the present invention provides a position / orientation estimation method having the following features, for example, with the configuration shown in FIG.

  That is, in the position / orientation estimation method for estimating the position / orientation value 124 of the distance sensor 106 by matching based on the environment map 101 prepared in advance and the sensing data 107 measured by the distance sensor 106, the position estimation method includes: , Means 150 for creating a first reference data group 103 and a second reference data group 105 for matching from the environment map 101, measured sensing data 107, first reference data group 103, and second In the position / orientation estimation system having the means 170 for estimating the position / orientation value 124 based on the reference data group 105, the means 150 for creating the reference data group creates the first reference data group 103 from the environment map 101. 102 and the difference between the second reference data group 105 and the first reference data group 103 The processing 170 created by the processing and the position / orientation estimating means 170 select necessary data from the first reference data group 103 and the second reference data group 105 based on the sensing data 107. A process 120, a process 121 for creating new evaluation data 122 by adding the selected reference data and the previously created evaluation data 122, and a position from the evaluation data 122 Processing 123 for obtaining the estimated posture value 124 is performed.

  According to the present invention, it is possible to further reduce the amount of calculation compared to the matching method based on the sensing data list.

  Embodiments of the present invention will be described below.

  FIG. 2 is a system configuration diagram showing the configuration of the position / orientation estimation system 10 according to an embodiment of the present invention. The position / orientation estimation system 10 includes a reference data creation server 20 and a mobile robot 30. The reference data creation server 20 is connected to a communication line 11 such as a LAN. The mobile robot 30 performs wireless communication with the base station 12 connected to the communication line 11 by a communication method such as a wireless LAN, and communicates with the reference data creation server 20 via the base station 12.

  The reference data creation server 20 creates a reference map and a later-described difference image from the two-dimensional environment map, and sends the created reference map and difference image to the mobile robot 30 via the communication line 11.

  The mobile robot 30 includes a wireless communication device 31, a position / orientation estimation device 32, a distance sensor 33, and a movement device 34. The wireless communication device 31 performs wireless communication with the base station 12, receives a reference map and a difference image from the reference data creation server 20 via the base station 12, and supplies them to the position / orientation estimation device 32. The position / orientation estimation device 32 uses the reference map and difference image supplied from the wireless communication device 31 and the sensing data measured by the distance sensor 33 to determine the position and orientation (orientation) of the mobile robot 30 in the environment map. Is estimated. The moving device 34 moves the mobile robot 30 according to a preset movement plan based on the position and orientation of the mobile robot 30 in the environment map calculated by the position and orientation estimation device 32.

  FIG. 3 is a block diagram illustrating an example of a functional configuration of the reference data creation server 20. The reference data creation server 20 includes an environment map storage unit 21, a reference map creation unit 22, a reference map storage unit 23, a difference image creation unit 24, a difference image storage unit 25, and a data transmission unit 26.

  Here, in the matching method based on the sensing data list described in the “Background Art” section, a method of performing not only translation but also estimation of rotation at the same time will be described. That is, it is assumed that the mobile robot simultaneously estimates the parameter for the degree of freedom θ related to rotation (posture) in addition to the degree of freedom (x, y) related to parallel movement.

  Also in the present embodiment, the reference map described in the description of the matching method based on the sensing data list in the “background art” column is used. That is, a reference map is prepared in advance for every value (d, φ) considered as sensing data. In the following description, the reference map for each value (d, φ) is referred to as a reference map of class (d, φ).

  Here, as shown in FIG. 4 (a), the meaning of the reference map 400 when given the reference map 400 of the class (d, -φ) will be described.

  The reference map 400 of the class (d, -φ) when the rotation direction θ of the mobile robot is 0 ° is used for the sensing data 401 (see FIG. 4B) of the distance d and the direction -φ. However, considering the rotational direction θ of the mobile robot, it can be interpreted as follows. That is, when sensing data 402 (see FIG. 4C) of distance d and direction (−φ−θ) is obtained, the direction of the mobile robot and the main direction of the environment map (in this embodiment, the x-axis direction). Is the case in which the entire sensing data is rotated by θ with respect to the environmental map (see FIG. 4D).

  In such a case, in the distance sensor in a non-rotating state (that is, θ = 0 °), sensing equivalent to that when the sensing data of the distance d and the direction −φ is obtained is performed. d, direction -φ, and the distance sensor (mobile robot) rotation direction θ = 0 ° sensing data, distance d, direction (-φ-θ), and distance sensor (mobile robot) In the calculation, the sensing data of the rotation θ can be handled in the same manner.

  On the other hand, as described in the “Background Art” column, a colored portion (a portion having a value of 1) on the reference map 400 of class (d, −φ) is (d, −φ) as sensing data. When the sensor value is observed, if the sensor origin is at this position, the evaluation value for the sensing data is 1. This was the case where the direction difference θ between the environment map and the mobile robot was 0 °. However, as mentioned above, taking into account the rotation of the mobile robot, the situation is that when the value of (d, -φ-θ) is observed as sensing data, the mobile robot Equivalent to rotating by θ.

Therefore, the meaning of the portion having a value of 1 on the reference map 400 can be interpreted in 360 ways as follows.
(0) When the direction θ of the mobile robot is θ = 0 ° and the value of (d, -φ) is observed as sensing data, if the sensor origin is at this position, the evaluation value for the sensing data is 1. become.
(1) When the direction θ of the mobile robot is θ = 1 ° and the value of (d, -φ-1 °) is observed as sensing data, if the sensor origin is at this position, evaluation of the sensing data The value becomes 1.
(2) When the direction θ of the mobile robot is θ = 2 ° and the value of (d, -φ-2 °) is observed as sensing data, if the sensor origin is at this position, evaluation of the sensing data The value becomes 1.
・
・
・
(359) When the direction θ of the mobile robot is θ = 359 ° and the value of (d, -φ-359 °) is observed as sensing data, if the sensor origin is at this position, evaluation of the sensing data The value becomes 1.

  If an evaluation value image is created according to the following procedure based on such a principle, matching that can simultaneously estimate the rotation direction θ in addition to the position (x, y) can be performed.

  First, a reference map is created in advance for each class represented by a polar coordinate expression in the form of (r, φ) as shown in FIG. Here, r and φ can take values of r = 0.1m, 0.2m, ..., 4.0m, φ = 0 °, 1 °, ..., 359 °, respectively, so all these combinations are arranged. . These reference maps are created before the mobile robot moves and after the environment map image of the room where the mobile robot moves is given. The creation method is as described in the description of the matching method based on the sensing data list in the section “Background Technology”.

Next, the mobile robot moves, and the sensing data group 430 shown in FIG. 14 becomes (d ₀ , -90 °), (d ₁ , -89 °), ..., (d ₁₈₀ , + 90 °). A method for estimating the position and orientation of the mobile robot from the sensing data and the reference map 230 shown in FIG. 5 will be described.
(1) First, it is assumed that the direction θ of the mobile robot is θ = 0 °.
(2) (d ₀ , -90 °), (d ₁ , -89 °), ..., (d ₁₈₀ , + 90 °) reference maps (181 total) in the series of Fig. 5 Select from the reference map group. When selecting, if the value of ψ in (d _i , ψ) is smaller than 0, 360 ° is added to ψ + 360 ° so that ψ is within the range of 0 ° ≦ ψ <360 °. To fit the value of. Even in this way, the meaning of the pointing angle does not change. This concept is used in the same way in the processing that handles all angles in the following description.
(3) An evaluation value image is created by adding all the selected 181 reference maps for each pixel. The pixel value at the position (x, y) in this evaluation value image becomes the evaluation value in the parameter (x, y, θ) (where θ = 0 °).
(4) Next, it is assumed that θ = 1 °.
(5) (d0, -90 ° -θ), (d1, -89 ° -θ), ..., (d180, + 90 ° -θ) reference maps (total 181) for each class (2 In the same manner as in (), a selection is made from the series of reference map groups in FIG.
(6) As in (3), an evaluation value image is created by adding these together for each pixel. The pixel value at the position (x, y) of this evaluation value image becomes the evaluation value in the parameter (x, y, θ).
(7) The value of θ is increased by 1 °, and the processes (5) and (6) are performed. This is repeated until θ = 359 °.
(8) Through the above processing, a total of 360 evaluation value images of θ = 0 °, 1 °,..., 359 ° are created. The combination (x, y, θ) of the position (x, y) on the environment map and the value of θ to which the evaluation value belongs is an estimated value.

  In the present invention, the method described above uses the method based on the sensing data list described in the “Background Art” section in the present invention to obtain the rotational direction θ of the mobile robot simultaneously with the parallel movement amount (x, y) of the mobile robot. Is the method. Using this method alone is not particularly effective compared to the method based on the sensing data list described in the “Background Art” section. However, in order to make the explanation of the method using the difference processing described below easier to understand, Detailed. This method is called a matching method including rotation based on the sensing data list.

  Next, a method using differential processing in the present invention will be described. In short, based on the concept of the matching method including the rotation based on the sensing data list described above, a difference process related to the reference map is added thereto.

  First, as shown in FIG. 6, consider creating a difference image 412 from two reference maps 410 and a reference map 411.

  Here, the difference image 412 is created by selecting a set in which the distance parameter r has the same value and the angle parameter φ has an adjacent value. That is, a reference map of class (r, ψ) and a reference map of class (r, ψ + 1 °) are selected (where r = 0.1, 0.2, ..., 4.0m, ψ = 0 ° , 1 °, ..., 359 °). The difference image created in this way is called a class (r, ψ + 1 °) difference image.

  The difference image G (r, ψ) of the class (r, ψ) can be calculated by the following Equation 1 using the reference map F (r, ψ) of the class (r, ψ).

G (r, ψ + 1 °) (x, y) = F (r, ψ + 1 °) (x, y) −F (r, ψ) (x, y) Equation 1
Note that (x, y) indicates coordinates in each image.

  As described above, r has a value of r = 0.1 m, 0.2 m, ..., 4.0 m, and ψ has a value of ψ = 0 °, 1 °, ..., 359 °. Since there are 360 patterns, a total of 40 × 359 difference images G are created.

  By the way, consider the reference maps of two classes (r, ψ + 1 °) and (r, ψ) composed of adjacent angles, ie, ψ and ψ + 1 ° angles, used when creating the difference image G. Since the former image is obtained by rotating the latter image slightly by 1 °, it has the property that the shape of the image is very similar. For this reason, when the difference image G is created based on the above equation, since the pixel values in the two images are often the same, the difference values of many parts of the pixels in the image are zero. .

In the present invention, for the difference image G, the pixel values of all the pixels are not held as the difference image G, but the pixel positions and pixel values are held as the difference image for pixels having pixel values other than 0. Specifically, assuming that the pixel value of the corresponding pixel is v _k , (x ₀ , y ₀ , v ₀ ), (x ₁ , y ₁ , v ₁ ), ..., (x _n , y _n , v _n ) shall be recorded. This brings about an effect that the number of times of reference to each pixel of the difference image G is reduced, that is, it is not necessary to refer to a pixel having a value of 0 in the difference image G. Therefore, it is possible to reduce the amount of data and the amount of calculation.

  Returning to FIG. 3, the description will be continued. The environmental map storage unit 21 stores, for example, the two-dimensional environmental map described with reference to FIG. The reference map creation unit 22 extracts r (r = 0.1 m, 0.2 m,..., 4.0 m in this embodiment) and φ (in this embodiment) from the environment map stored in the environment map storage unit 21. , Φ = 0 °, 1 °,..., 359 °), a total of 40 × 360 reference maps are created, and the created reference maps are stored in the reference map storage unit 23. The reference map storage unit 23 stores the reference map 230 described with reference to FIG.

  The difference image creation unit 24 creates a difference image of each class using the reference map stored in the reference map storage unit 23 based on Equation 1 described above, and uses the created difference image as the difference image storage unit 25. To store. For example, as illustrated in FIG. 7, the difference image storage unit 25 includes, for each class 250, a pixel position 251 indicating the position of a pixel whose pixel value is not 0 in the difference image corresponding to the class, and the pixel position 251. The pixel value 252 of the pixel at is stored.

  The data transmission unit 26 receives the environment map stored in the environment map storage unit 21, the reference map stored in the reference map storage unit 23, and the difference image stored in the difference image storage unit 25 as the communication line 11. Is transmitted to the mobile robot 30.

  FIG. 8 is a block diagram illustrating an example of a functional configuration of the position / orientation estimation apparatus 32. The position / orientation estimation apparatus 32 includes a data acquisition unit 320, a data storage unit 321, a first evaluation image creation unit 322, an integrated difference image creation unit 323, a first evaluation value calculation unit 324, and a second evaluation image creation unit 325. A position / orientation estimation unit 326, an evaluation value storage unit 327, and a second evaluation value calculation unit 328.

Here, a method for performing matching using a difference image in the present invention will be described. Here, reference to each class (r _j , ψ _k ) (r _j = 0.1m, 0.2m, 0.3m, ..., 4.0m, ψ _k = 0 °, 1 °, ..., 359 °) Map and difference image of class (r _j , ψ _k ) (r _j = 0.1m, 0.2m, 0.3m, ..., 4.0m, ψ _k = 1 °, 2 °, ..., 359 °) Are created in advance, and 181 data of (d ₀ , -90 °), (d ₁ , -89 °), ..., (d ₁₈₀ , + 90 °) are obtained as sensing data. Assume that.
(1) First, it is assumed that the direction θ of the mobile robot is θ = 0 °.
(2) A series of reference maps of (d ₀ , -90 °), (d ₁ , -89 °), ..., (d ₁₈₀ , + 90 °) reference maps (total of 181) Choose from. As described above, the angle is set within the range of 0 ° to less than 360 ° by appropriately adding 360 °.
(3) With respect to all the selected 181 reference maps, an evaluation value image is created by adding the reference maps for each pixel. The pixel value at the position (x, y) in this evaluation value image becomes the evaluation value in the parameter (x, y, θ) (where θ = 0 °).
(4) Next, it is assumed that θ = 1 °.
(5) Reference maps for each class of (d ₀ , -90 ° -θ), (d ₁ , -89 ° -θ), ..., (d ₁₈₀ , + 90 ° -θ) (181 in total) Are selected from the difference image group instead of the reference map.
(6) For all the selected difference images, image data is created by adding the difference images for each pixel. The image data created in this way is called an integrated difference image.
(7) Image data is created by adding the created integrated difference image and the evaluation value image created at the previous time, ie, (3) when θ = 0 °, for each pixel. This is an evaluation value image when the rotation angle of the mobile robot is θ. That is, the pixel value at the position (x, y) of the evaluation value image becomes the evaluation value in the parameter (x, y, θ).
(8) Increase the value of θ by 1 ° and perform the processing of (5), (6), and (7). This is repeated until θ = 359 °.
(9) Through the above processing, a total of 360 evaluation value images of θ = 0 °, 1 °, ..., 359 ° are created. Among these, the pixel with the largest value is found, and A combination (x, y, θ) of the position (x, y) on the image and the value of θ to which the image belongs becomes an estimated value of the parameter.

In the method using the difference image according to the present invention, the number of image data used at the time of calculation does not change compared to the matching method including rotation based on the sensing data list. Specifically, in the matching method including rotation based on the sensing data list, (d ₀ , -90 ° -θ), (d ₁ ) when θ = 0 °, 1 °, ..., 359 ° respectively. , -89 ° -θ),..., (D ₁₈₀ , + 90 ° -θ), a total of 181 different class reference maps are used, and the total number of images handled is 181 × 360.

On the other hand, in the method using the differential image according to the present invention, (d ₀ , −90 °), (d ₁ , −89 °), ..., (d ₁₈₀ , + 90 ° when θ = 0 °. ) For a total of 181 different class reference maps and (d ₀ , -90 ° -θ), (d ₁ , -89 ° for θ = 1 °, 2 °, ..., 359 ° Since a total of 181 different difference images (−θ),..., (d ₁₈₀ , + 90 ° −θ) are used, the total number of images handled is 181 × (1 + 359).

  As described above, the number of images used for the calculation is the same as the matching method including rotation based on the sensing data list and the method using the difference image according to the present invention. However, when attention is paid to each image data, the difference image is greatly reduced compared to the reference map in terms of the number of pixels having a value other than 0, that is, the number of pixels used in the calculation. . Therefore, the number of pixels used for calculation is reduced by the same amount as the reduction, and the calculation processing amount is also reduced in proportion thereto.

This is due to the property that most of the pixel values in the difference image are zero because the two-level reference maps of adjacent angles used when creating the difference image are very similar It is. If the number of pixels having a non-zero value in the difference image is, for example, 1/3 of the number of pixels having a non-zero value in the original reference map, the pixel value reference calculation in the matching method is performed. The number of times is as follows.
181 × 360 × 101 × 101 × 1/2 × 1/3 (times)
Here, the meanings of the numerical values “181”, “360”, “101”, “101”, and “1/2” are the same as in the matching method based on the sensing data list. The newly added value “1/3” is the reduction ratio of the number of pixels having a value other than 0 as described above. That is, in the method using the difference processing in the present invention, the number of reference operations is reduced by the number. This is the effect of the difference processing in the present invention.

  Returning to FIG. 8, the description of the functional configuration of the position / orientation estimation apparatus 32 will be continued. The data acquisition unit 320 acquires the environment map, the reference map of each class, and the difference image of each class from the reference data creation server 20 via the wireless communication device 31, and acquires the acquired environment map, reference map, and difference image. Is stored in the data storage unit 321.

The first evaluation image creation unit 322 acquires 181 sensing data (d _i , φ _i ) (i = 0, 1,..., 180) from the distance sensor 33. Then, the first evaluation image creation unit 322 has the same distance d _i and direction φ _i indicated by the sensing data (d _i , φ _i ) for each acquired sensing data (d _i , φ _i ). A reference map (d _i , φ _i ) corresponding to the distance and direction is extracted from the data storage unit 321. Then, the first evaluation image creation unit 322 adds the pixel values of the extracted 181 reference maps (d _i , φ _i ) for each pixel, thereby forming an evaluation image composed of the added pixel values ( θ ₀ ) is created. In the evaluation image (θ ₀ ), an angle formed by the direction of the mobile robot 30 and a predetermined direction in the environment map (in this embodiment, the x-axis direction in FIG. 16) is θ ₀ (in this embodiment, θ ₀ = 0). °) means an evaluation image assuming that

The first evaluation value calculation unit 324 uses the pixel value at the position (x, y) in the evaluation image (θ ₀ ) created by the first evaluation image creation unit 322 as (x, y, θ _0). ) Is stored in the evaluation value storage unit 327 as an evaluation value. In the evaluation value storage unit 327, for example, as shown in FIG. 9, the evaluation value 3273 of the pixel at the position in association with the x coordinate 3270 and the y coordinate 3271 indicating the position of the pixel in the environment map, A direction 3272 indicating the direction of the mobile robot 30 with respect to a predetermined direction in the environment map assumed when the evaluation value 3273 is calculated is stored.

Integrated differential image producing section 323, the distance 181 pieces of measured data _{(d i, φ i -θ j} ) measured by the sensor 33 for each are shown in the sensing data _{(d i, φ i -θ j} ) A difference image (d _i , φ _i −θ _j ) corresponding to the same distance and direction as the distance d _i and the direction φ _i −θ _j is extracted from the data storage unit 321. Then, the integrated difference image creation unit 323 adds the pixel values of the extracted 181 difference images (d _i , φ _i −θ _j ) for each pixel, and thereby the integrated difference image composed of the added pixel values. Create an image (θ _j ). The integrated difference image (θ _j ) means an integrated difference image when it is assumed that the angle between the direction of the mobile robot 30 and a predetermined direction in the environment map is θ _j .

The second evaluation image creation unit 325 creates an evaluation image (θ _j ) by adding the evaluation image (θ _j−1 ) and the integrated difference image (θ _j ) for each pixel. The second evaluation value calculation unit 328 calculates the pixel value at the position (x, y) in the evaluation image (θ _j ) created by the second evaluation image creation unit 325 as (x, y, θ _j ) Is stored in the evaluation value storage unit 327 as an evaluation value.

Note that the integrated difference image creation unit 323, the first evaluation value calculation unit 324, and the second evaluation image creation unit 325, after the evaluation image (θ ₀ ) is created by the first evaluation image creation unit 322, From j = 1 to j = m-1, a process for creating an integrated difference image (θ _j ), a process for creating an evaluation image (θ _j ), and the position of (x, y) in the evaluation image (θ _j ) The process of storing the evaluation value of the pixel in each is executed.

  The position / orientation estimation unit 326 refers to the evaluation value storage unit 327, identifies (x, y, θ) associated with the maximum evaluation value, and the angle formed with the predetermined direction of the environmental map is θ. The direction and the position of the mobile robot 30 are estimated at the position (x, y) in the environment map, and the estimated direction and position are notified to the moving device 34.

  FIG. 10 is a flowchart showing an example of the operation of the reference data creation server 20. For example, the reference data creation server 20 starts the operation shown in this flowchart at a predetermined timing such as when the environment map is stored in the environment map storage unit 21.

First, the reference map creating unit 22 initializes the value of j to 1 and the value of k to 0 (S100), and refers to the environment map storage unit 21 to class (r _j , ψ _k) from the environment map. ) Is created, and the created reference map is stored in the reference map storage unit 23 (S101). Then, the reference map creation unit 22 determines whether or not the value of j is 40. If the value of j is less than 40 (S102: No), the value of j is incremented by 1 (S103), and is shown again in step S101. Execute the process.

  When the value of j is 40 (S102: Yes), the reference map creation unit 22 initializes the value of j to 0, increases the value of k by 1 (S104), and is the value of k greater than 181? It is determined whether or not (S105). When the value of k is 181 or less (S105: No), the reference map creation unit 22 executes the process shown in step S101 again.

When the value of k is larger than 181 (S105: Yes), the difference image creation unit 24 initializes the values of j and k to 1 (S106), and refers to the reference map storage unit 23 to obtain the following formula 2. Is used to calculate a difference image in the class (r _j , ψ _k ), and the calculated difference image (r _j , ψ _k ) is stored in the difference image storage unit 25 (S107).

Difference image (r _j , ψ _k ) = reference map (r _j , ψ _k ) −reference map (r _j , ψ _k−1 ) Equation 2
Next, the difference image creation unit 24 determines whether or not the value of k is 181. If the value of k is less than 181 (S108: No), the value of k is increased by 1 (S109), and the step again The process shown in S107 is executed. When the value of k is 181 (S108: Yes), the difference image creation unit 24 increments the value of j by 1 and initializes the value of k to 1 (S110), and whether the value of j is larger than 40 or not. Is determined (S111).

  When the value of j is 40 or less (S111: No), the difference image creation unit 24 executes the process shown in step S107 again. When the value of j is larger than 40 (S111: Yes), the data transmission unit 26 stores the environmental map stored in the environmental map storage unit 21, the reference map stored in the reference map storage unit 23, and the difference image storage. The difference image stored in the unit 25 is transmitted to the mobile robot 30 via the communication line 11 (S112), and the reference data creation server 20 ends the operation shown in this flowchart.

  FIG. 11 is a flowchart showing an example of the operation of the position / orientation estimation apparatus 32. The mobile robot 30 starts the operation shown in this flowchart at a predetermined timing after receiving the environment map, the reference map, and the difference image from the reference data creation server 20.

First, the first evaluation image creation unit 322 initializes the value of i to 0 (S200), and the reference map (d _i , φ _i ) corresponding to the sensing data (d _i , φ _i ) measured by the distance sensor 33. _i ) is extracted from the data storage unit 321 (S201). Then, the first evaluation image creation unit 322 determines whether or not the value of i is 180. If the value of i is less than 180 (S202: No), the value of i is increased by 1 ( S203), the process shown in step S201 is executed again.

When the value of i is 180 (S202: Yes), the first evaluation image creation unit 322 adds the extracted reference map (d _i , φ _i ) for each pixel, thereby evaluating the evaluation image (θ ₀ ). Is created (S204). Then, the first evaluation value calculation unit 324 converts the pixel value at the position (x, y) in the evaluation image (θ ₀ ) created by the first evaluation image creation unit 322 to (x, y, The evaluation value in θ ₀ ) is stored in the evaluation value storage unit 327 (S205).

Next, the integrated difference image creation unit 323, the second evaluation image creation unit 325, and the second evaluation value calculation unit 328 initialize the i value to 0 and the j value to 1, respectively (S206). ). Then, the integrated difference image creation unit 323 obtains a difference image (d _i , φ _i −θ _j ) corresponding to the sensing data (d _i , φ _i −θ _j ) measured by the distance sensor 33 from the data storage unit 321. It is extracted (S207), and it is determined whether or not the value of i is 180 (S208). If the value of i is less than 180 (S208: No), the integrated difference image creation unit 323 increases the value of i by 1 (S203), and executes the process shown in step S207 again.

When the value of i is 180 (S208: Yes), the integrated difference image creation unit 323 adds the pixel values of the extracted 181 difference images (d _i , φ _i −θ _j ) for each pixel. Then, an integrated difference image (θ _j ) composed of the added pixel values is created (S210). Then, the second evaluation image creation unit 325 creates an evaluation image (θ _j ) by adding the evaluation image (θ _j−1 ) and the integrated difference image (θ _j ) for each pixel ( S211).

Next, the second evaluation value calculation unit 328 calculates the pixel value at the position (x, y) in the evaluation image (θ _j ) created by the second evaluation image creation unit 325 as (x, y , θ _j ) is stored in the evaluation value storage unit 327 as an evaluation value (S212), and it is determined whether or not the value of j is 359 (S213). When the value of j is less than 359 (S213: No), the integrated difference image creation unit 323 increases the value of j by 1 (S214), and executes the process shown in step S207 again.

  When the value of j is 359 (S213: Yes), the position / orientation estimation unit 326 refers to the evaluation value storage unit 327 and identifies (x, y, θ) associated with the maximum evaluation value. Then, the direction and position of the mobile robot 30 are estimated at the position (x, y) in the environment map with the angle formed with the predetermined direction of the environment map being θ, and the position and orientation estimation device 32 is Then, the operation shown in this flowchart is finished.

  FIG. 12 is a hardware configuration diagram illustrating an example of a hardware configuration of the computer 40 that realizes the functions of the reference data creation server 20 or the position / orientation estimation apparatus 32. The computer 40 includes a central processing unit (CPU) 41, a random access memory (RAM) 42, a read only memory (ROM) 43, a hard disk drive (HDD) 44, a communication interface (I / F) 45, an input / output interface (I). / F) 46 and a media interface (I / F) 47.

  The CPU 41 operates based on a program stored in the ROM 43 or the HDD 44 and controls each part. The ROM 43 stores a boot program executed by the CPU 41 when the computer 40 is started up, a program depending on the hardware of the computer 40, and the like. The HDD 44 stores a program executed by the CPU 41. The communication interface 45 receives data from other devices via a communication line and sends the data to the CPU 41, and transmits data generated by the CPU 41 to other devices via the communication line.

  The CPU 41 controls an output device such as a monitor and an input device such as a keyboard and a mouse via the input / output interface 46. The CPU 41 acquires data from the input device via the input / output interface 46. Further, the CPU 41 outputs the generated data to the output device via the input / output interface 46. When the computer 40 functions as the position / orientation estimation device 32, the computer 40 is not provided with the communication interface 45, and the computer 40 performs communication via the wireless communication device 31 connected to the input / output interface 46. When the computer 40 functions as the position / orientation estimation device 32, data exchange with the distance sensor 33 and the moving device 34 is performed via the input / output interface 46.

  The media interface 47 reads a program or data stored in the recording medium 48 and provides it to the RAM 42. A program provided to the CPU 41 via the RAM 42 is stored in the recording medium 48. The program is read from the recording medium 48, installed in the computer 40 via the RAM 42, and executed by the CPU 41. The recording medium 48 is, for example, an optical recording medium such as a DVD (Digital Versatile Disk) or PD (Phase change rewritable disk), a magneto-optical recording medium such as an MO (Magneto-Optical disk), a tape medium, a magnetic recording medium, or a semiconductor memory. Etc.

  When the computer 40 functions as the reference data creation server 20, the program installed and executed on the computer 40 includes the environment map storage unit 21, the reference map creation unit 22, the reference map storage unit 23, and difference image creation. Function as the unit 24, the difference image storage unit 25, and the data transmission unit 26.

  Further, when the computer 40 functions as the position / orientation estimation device 32, a program installed and executed on the computer 40 includes the data acquisition unit 320, the data storage unit 321, the first evaluation image creation unit 322, The integrated difference image creation unit 323, the first evaluation value calculation unit 324, the second evaluation image creation unit 325, the position / orientation estimation unit 326, the evaluation value storage unit 327, and the second evaluation value calculation unit 328 are caused to function.

  The computer 40 reads these programs from the recording medium 48 and executes them, but as another example, these programs may be acquired from other devices via a communication medium. The communication medium refers to a wired or wireless communication line, or a digital signal or a carrier wave that propagates through the communication line.

  The embodiment of the present invention has been described above.

  As is clear from the above description, according to the position / orientation estimation system 10 of the present embodiment, it is possible to realize a further reduction in the amount of calculation compared to the matching method based on the sensing data list.

  Here, based on the description given so far, the correspondence between the configuration of the present invention described with reference to FIG. 1 in the column of “Means for Solving the Problems” and the above-described embodiment will be supplemented. .

  In FIG. 1, the environment map 101 is a digital image representing the presence of an obstacle at a height cross section measured by the distance sensor 106 as described in the section “Background Art”. Here, a place where an obstacle does not exist is represented as a pixel value 0 (white), and a place where an obstacle exists is represented as a pixel value 1 (black).

  As described in the “Background Art” section, the distance sensor 106 can measure the distance to the obstacle in a plurality of predetermined directions around the user. Here, it is assumed that the measurement is performed in a total of 181 different directions every 1 ° from −90 ° to + 90 ° with respect to the front.

As described in the “Background Art” section, the sensing data 107 is obtained by detecting the position of the obstacle as a combination of an angle and a distance (d ₀ , −90 °), (d ₁ , −89 °). ),..., (D ₁₈₀ , + 90 °) are represented as a total of 181 polar coordinate data. Further, d ₀ , d ₁ ,..., D ₁₈₀ are quantized as any value of 0.1 m, 0.2 m, 0.3 m,.

  The position / orientation estimated value 124 is expressed as a total of three parameters (x, y, θ) related to the position and direction since it is assumed that the mobile robot performs a planar motion as described in the “Background Art” section. Is.

First, a method for creating the first reference data group 103 for matching from the environment map in the reference data group creating unit 150 in FIG. 1 will be described. Here, the first reference data group indicates a series of reference maps described in the “Background Art” section. Specifically, (r _j , ψ _k ) (where, r _j = 0.1 m, 0.2 m, 0.3 m, ..., 4.0 m, 40 ways, ψ _k = 0 °, 1 °, ..., 360 classes (359 °), so the total is 40 × 360. This data is created by moving the environment map in the direction of (x, y). Note that x = _rj · cos (ψ _k ) and y = r _j · sin (ψ _k ).

Each pixel is quantized so as to indicate a region of 10 cm × 10 cm in the actual environment. Furthermore, this reference map is not the image itself, but the data that enumerates the coordinate values of pixels having a value of 1 on the image as (x ₀ , y ₀ ), (x ₁ , y ₁ ), ... , (x _n, y _n ).

Next, a method of creating the second reference data group 105 from the first reference data group in the means 150 for creating the reference data group in FIG. 1 will be described. Here, the second reference data group indicates a series of difference images described in the first embodiment. Specifically, (r _j , ψ _k ) (where r _j = 0.1 m, 0.2 m, 0.3 m, ..., 4.0 m, 40 ways, ψ _k = 1 °, 2 °, ..., Since it is created as 359 classes of 359 °, the total is 40 × 359. This data is a combination of two of the adjacent classes for the angle parameter in the reference map described above, and this image pixel It is created by performing difference processing for each.

That is, a difference image of class (r _j , ψ + 1 °) is created from the reference map of class (r _j , ψ + 1 °) and class (r _j , ψ). The specific production method is as described in the first half of the best mode for carrying out the invention. This difference image is not the image itself but the coordinate values of pixels having values other than 0 on the image and the data enumerating the values as (x ₀ , y ₀ v ₀ ), (x ₁ , y ₁ , v ₁ ), ..., (x _n , y _n , v _n ).

  Next, processing for selecting necessary data from the first reference data group and the second reference data group based on the sensing data in the means 170 for estimating the position and orientation in FIG. 1 will be described. Here, assuming that the rotation of the mobile robot is θ, based on a value calculated from this value and sensing data, a first reference data group, that is, a reference map of each class, and a second reference data group, that is, An appropriate class is selected from the difference image of each class. Multiple items will be selected. The specific selection method is as described in the first half of the best mode for carrying out the invention.

  Next, processing 121 for creating data 122 for evaluation by adding selected data in the means 170 for estimating the position and orientation of FIG. 1 will be described. Here, for each assumed value of the rotation θ of the mobile robot, the reference map of each class selected above and the difference image are added for each pixel, and further created as the rotation (θ-1 °) of the mobile robot. The obtained evaluation value images are added together to obtain an evaluation value image of the rotation θ of the mobile robot. The specific method is as described in the first half of the best mode for carrying out the invention. This evaluation value image becomes data for evaluation in FIG.

  Next, processing 123 for obtaining a position / orientation estimation value from data for evaluation in the means 170 for estimating the position / orientation of FIG. 1 will be described. This is because the pixel having the maximum value is found from the evaluation value image calculated for each rotation θ of each mobile robot obtained above, and the rotation angle θ to which the pixel belongs and the position of the pixel on the image ( The position / orientation estimation value is obtained as (x, y, θ) from x, y). The specific method is as described in the first half of the best mode for carrying out the invention.

  Further, the present invention is not limited to the above-described embodiment, and various modifications can be made within the scope of the gist thereof.

  For example, in the above-described embodiment, the position / orientation estimation device 32 uses both the reference map and the difference image created in advance by the reference data creation server 20 during parameter estimation, but the present invention is not limited to this. As another form, the position and orientation estimation device 32 may estimate each parameter using a difference image created in advance by the reference data creation server 20 and a part of the reference map.

  For example, the reference data creation server 20 has a class of ψ = 0 ° as a reference map, that is, class (0.1 m, 0 °), class (0.2 m, 0 °), ..., class (4.0 m, 0 °). The position / orientation estimation apparatus 32 stores the reference map of the class created by the reference data creation server 20 in the data storage unit 321. This is a memory amount of 1/360 as compared with the case where reference maps of all classes of ψ = 0 °, 1 °,..., 359 ° are recorded in the above-described embodiment. As for the difference image, the reference data creation server 20 creates the difference image for all classes as in the above-described embodiment, and the position / orientation estimation device 32 creates all the classes created by the reference data creation server 20. Are stored in the data storage unit 321.

Based on this preparation, the first evaluation image creation unit 322 creates an evaluation value image when the direction θ is assumed to be θ = 0 ° according to the following steps.
(1) Set θ = 0 °.
(2) For the sensing data (d ₀ , -90 °), (d ₁ , -89 °), ..., (d ₁₈₀ , + 90 °), the distance parameter is the respective value and the angle parameter is 0 ° Select the reference map of the class replaced with. That is, a reference map of a class of (d ₀ , 0 °), (d ₁ , 0 °), ..., (d ₁₈₀ , 0 °) is selected.
(3) For these data, add the difference images until the angle parameter is that of the sensing data. The added image data becomes a reference map of the class. For example, for the sensing data of (d ₀ , -90 °), after converting from -90 ° to -90 ° + 360 ° = + 270 °, the following calculation is performed, which is the purpose. Create a reference map of class (d ₀ , + 270 °).

F (d ₀ , 270 °) = F (d ₀ , 0 °) + G (d ₀ , 1 °) + G (d ₀ , 2 °) + ... + G (d ₀ , 270 °)
Expression 3 where F (d, ψ) and G (d, ψ) are a reference map of class (d, ψ) and a difference image of class (d, ψ), respectively.

  The reference map of each class when θ = 0 ° is obtained by the above processing, and thereafter, the target estimation is performed by adding the integrated difference image at each angle to the reference map. A value will be required. By doing so, a process for creating a reference map of each class corresponding to the sensing data is newly required, but it is not necessary to store most of the reference map in the data storage unit 321. Can be reduced.

  In the above-described embodiment, the mobile robot 30 acquires the environment map, the reference map, and the difference image from the reference data creation server 20 through wired or wireless communication. However, as another form, the mobile robot 30 is allowed. You may make it acquire these data from the reference data creation server 20 via a portable recording medium.

  In the above-described embodiment, the position / orientation estimation system 10 includes the reference data creation server 20 and the mobile robot 30. However, as another form, the function of the reference data creation server 20 is provided in the mobile robot 30. The mobile robot 30 alone may be configured to realize the function of the position / orientation estimation system 10.

  In addition, various specific numerical values used in the above-described embodiments are provisionally set to help understanding of the description, and do not limit the use of the values when the present invention is implemented. . For example, in the above-described embodiment, the angular resolution is 1 °. However, depending on the specifications of the system to which the present invention is applied and the required accuracy, the angle resolution is 0.5 °, 0.1 °, etc. Also good.

It is a conceptual diagram for demonstrating an example of the whole structure of the position and orientation estimation system 10 of this invention. 1 is a system configuration diagram illustrating a configuration of a position and orientation estimation system 10 according to an embodiment of the present invention. 3 is a block diagram illustrating an example of a functional configuration of a reference data creation server 20. FIG. It is a conceptual diagram for demonstrating the meaning of the reference map. 5 is a conceptual diagram for explaining an example of a data structure stored in a reference map storage unit 23. FIG. It is a conceptual diagram for demonstrating the creation process of the difference image 412. FIG. 6 is a diagram illustrating an example of a data structure stored in a difference image storage unit 25. FIG. 3 is a block diagram illustrating an example of a functional configuration of a position / orientation estimation apparatus 32. FIG. It is a figure which shows an example of the data structure stored in the evaluation value storage part 327. 4 is a flowchart illustrating an example of an operation of a reference data creation server 20. 5 is a flowchart showing an example of the operation of a position / orientation estimation apparatus 32. 2 is a hardware configuration diagram illustrating an example of a computer 40 that realizes the function of the reference data creation server 20. FIG. FIG. 11 is a conceptual diagram for explaining an outline of the operation of the mobile robot 420. It is a figure which shows an example of the sensing data group 430 measured by the distance sensor 421. It is a figure which shows an example of the environment map. It is a conceptual diagram for demonstrating the process which pinpoints the position of a mobile robot. It is a conceptual diagram which shows an example of the sensing data group 460 and the environment map 463. It is a conceptual diagram for demonstrating the process of the matching method by a basic method. It is the figure which extracted the case where the 0th sensing data 470 and the black pixel in an environment map correspond. It is the figure which plotted the position of the sensor origin 481 in case the 0th sensing data 470 and the black pixel in an environment map correspond. It is a conceptual diagram for demonstrating the relationship between FIG. 20 and an environmental map. It is a conceptual diagram for demonstrating the relationship between each sensing data and a reference map. It is a conceptual diagram for demonstrating the process in which an evaluation image is produced. It is a figure which shows an example of the evaluation image 510. FIG.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 ... Position and orientation estimation system, 101 ... Environmental map, 102 ... Creation process of 1st reference data group, 103 ... 1st reference data group, 104 ... 2nd reference data Group creation processing, 105 ... second reference data group, 106 ... distance sensor, 107 ... sensing data, 120 ... reference data selection processing, 121 ... addition processing, 122 ... Data for evaluation, 123... Processing for obtaining position and orientation estimation values, 124... Position and orientation estimation values, 150... Means for creating a reference data group, 170. Means: 11 ... communication line, 12 ... base station, 20 ... reference data creation server, 21 ... environment map storage, 22 ... reference map creation, 23 ... reference map storage Part, 24... Difference image creation part, DESCRIPTION OF SYMBOLS 5 ... Difference image storage part, 26 ... Data transmission part, 30 ... Mobile robot, 31 ... Wireless communication apparatus, 32 ... Position and orientation estimation apparatus, 320 ... Data acquisition part, 321 ... Data storage unit, 322 ... First evaluation image creation unit, 323 ... Integrated difference image creation unit, 324 ... First evaluation value calculation unit, 325 ... Second evaluation image Creation unit, 326 ... Position and orientation estimation unit, 327 ... Evaluation value storage unit, 328 ... Second evaluation value calculation unit, 33 ... Distance sensor, 34 ... Moving device

Claims (4)

Different angle between the main direction of the distance sensor, the order in [psi ₀ angle is smaller with the main _{direction, ψ 1, ··· ψ m-} 1 to m-number of directions ψ _{k (k} = 0,1, · A vector A (r, ψ _k ) is defined for each combination of direction ψ _k and distance r at m-1) and a different distance r from the distance sensor;
For each vector A (r, ψ _k ), a two-dimensional environment map composed of a plurality of pixels is set in a direction opposite to the direction indicated by the vector A (r, ψ _k ). , ψ _k ) to create a reference map (r, ψ _k ) by translating by the distance indicated by
Sensing data measured by the reference sensor (r, ψ _k ) and the distance sensor, and n directions φ _i (i = 0, 1, ··· n-1), and the distance from the distance sensor in each direction phi _i to the object d _{i (i} = 0,1, sensing indicating the ··· n-1) data (d _i, phi _i ) And a position and orientation estimation method in a position and orientation estimation system for estimating the position and orientation of the device in which the distance sensor is mounted in the environment map,
The position and orientation estimation system includes:
A reference data creation server;
A position and orientation estimation device mounted on a device on which the distance sensor is mounted;
The reference data creation server is
A reference map creating step for creating a reference map (r, ψ _k ) (k = 0, 1, ... m-1) from the environmental map;
With reference to the created reference map (r, ψ _k ), the reference map in the adjacent direction is referred to by the reference map (r, ψ _k ) −reference map (r, ψ _k−1 ) at the same distance r. A difference is calculated for each pixel, and a difference image (r, ψ _k ) (k = 1, 2,..., M−1) that collects pixels having pixel values other than 0 in the calculated difference for each pixel. Execute the difference image creation step to be created,
The position / orientation estimation device comprises:
For each of n pieces of sensing data (d _i , φ _i ) (i = 0, 1,... N-1) measured by the distance sensor, the sensing data (d _i , φ _i ) is indicated. A reference map (d _i , φ _i ) corresponding to the same distance and direction as the distance d _i and direction φ _i is extracted from the reference map calculated by the reference data creation server, and extracted n references By adding the pixel values of the map (d _i , φ _i ) for each pixel, the orientation of the device in which the distance sensor is mounted and the main direction of the environmental map, which is composed of the added pixel values, is formed. A first evaluation image creating step for creating an evaluation image (θ ₀ ) having an angle of θ ₀ ;
A first evaluation value calculating step of calculating a pixel value at a position of (x, y) in the evaluation image (θ ₀ ) as an evaluation value in (x, y, θ ₀ );
For each of the n sensing data (d _i , φ _i −θ _j ) measured by the distance sensor from j = 1 to j = m−1, the sensing data (d _i , φ _i −θ _j ) The difference image (d _i , φ _i -θ _j ) corresponding to the same distance and direction as the distance d _i and the direction φ _i -θ _j shown in FIG. The distance sensor configured to extract and add the pixel values of the extracted n difference images G (d _i , φ _i −θ _j ) for each pixel is mounted. Create an integrated difference image (θ _j ) with the angle between the orientation of the device and the main direction of the environment map as θ _j, and evaluate image (θ _j-1 ) and integrated difference image (θψ _j ), By adding for each pixel, an evaluation image (θ _j ) is created, and the pixel value at the position of (x, y) in the created evaluation image (θ _j ) is set to (x, y, θ _j ) As an evaluation value A second evaluation value calculation step of leaving,
Among the evaluation values calculated in the first evaluation value calculation step and the evaluation values calculated in the second evaluation value calculation step, (x, y, θ) corresponding to the maximum evaluation value is specified. A position and orientation estimation step for estimating an orientation and a position of a device in which the distance sensor is mounted at a position (x, y) in the environment map, with an angle formed with the main direction of the environment map being a direction θ; A position / orientation estimation method comprising: Different angle between the main direction of the distance sensor, the order in [psi ₀ angle is smaller with the main _{direction, ψ 1, ··· ψ m-} 1 to m-number of directions ψ _{k (k} = 0,1, · A vector A (r, ψ _k ) is defined for each combination of direction ψ _k and distance r at m-1) and a different distance r from the distance sensor;
For each vector A (r, ψ _k ), a two-dimensional environment map composed of a plurality of pixels is set in a direction opposite to the direction indicated by the vector A (r, ψ _k ). , ψ _k ) to create a reference map (r, ψ _k ) by translating by the distance indicated by
Sensing data measured by the reference sensor (r, ψ _k ) and the distance sensor, and n directions φ _i (i = 0, 1, ··· n-1), and the distance from the distance sensor in each direction phi _i to the object d _{i (i} = 0,1, sensing indicating the ··· n-1) data (d _i, phi _i ) And a position and orientation estimation system for estimating the position and orientation of the device on which the distance sensor is mounted in the environmental map,
A reference data creation server;
A position and orientation estimation device mounted on a device on which the distance sensor is mounted;
The reference data creation server is
A reference map creating means for creating a reference map (r, ψ _k ) (k = 0, 1, ... m-1) from the environmental map;
With reference to the reference map (r, ψ _k ) created by the reference map creating means, the reference map (r, ψ _k ) −reference map (r, ψ _k−1 ) for the same distance r, A difference between reference maps in adjacent directions is calculated for each pixel, and a difference image (r, ψ _k ) (k = 1, 2,...) That collects pixels having pixel values other than 0 in the calculated difference for each pixel. ..M-1) and a differential image creating means for creating
The position / orientation estimation apparatus includes:
For each of n pieces of sensing data (d _i , φ _i ) (i = 0, 1,... N-1) measured by the distance sensor, the sensing data (d _i , φ _i ) is indicated. A reference map (d _i , φ _i ) corresponding to the same distance and direction as the distance d _i and direction φ _i is extracted from the reference map calculated by the reference data creation server, and extracted n references By adding the pixel values of the map (d _i , φ _i ) for each pixel, the orientation of the device in which the distance sensor is mounted and the main direction of the environmental map, which is composed of the added pixel values, is formed. A first evaluation image creating means for creating an evaluation image (θ ₀ ) having an angle of θ ₀ ;
A first evaluation value calculating means for calculating a pixel value at a position of (x, y) in the evaluation image (θ ₀ ) as an evaluation value in (x, y, θ ₀ );
For each of the n sensing data (d _i , φ _i −θ _j ) measured by the distance sensor from j = 1 to j = m−1, the sensing data (d _i , φ _i −θ _j ) The difference image (d _i , φ _i -θ _j ) corresponding to the same distance and direction as the distance d _i and the direction φ _i -θ _j shown in FIG. An apparatus on which the distance sensor is mounted, which is configured by adding the pixel values of the extracted n extracted difference images (d _i , φ _i -θ _j ) for each pixel. An integrated difference image creating means for creating an integrated difference image (θ _j ) with θ _j as an angle formed between the orientation of the environment map and the main direction of the environment map;
From j = 1 to j = m−1, an evaluation image (θ _j ) is created by adding the evaluation image (θ _j-1 ) and the integrated difference image (θ _j ) for each pixel. Evaluation image creation means,
A second pixel value is calculated as an evaluation value at (x, y, θ _j ) from j = 1 to j = m−1 at the position (x, y) in the evaluation image (θ _j ). An evaluation value calculating means;
Among the evaluation values calculated by the first evaluation value calculation means and the evaluation values calculated by the second evaluation value calculation means, (x, y, θ) corresponding to the maximum evaluation value is A position and orientation estimation that identifies and estimates the orientation and position of the device in which the distance sensor is mounted at the position (x, y) in the environment map with an angle of θ with the main direction of the environment map And a position and orientation estimation system. Different angle between the main direction of the distance sensor, the order in [psi ₀ angle is smaller with the main _{direction, ψ 1, ··· ψ m-} 1 to m-number of directions ψ _{k (k} = 0,1, · A vector A (r, ψ _k ) is defined for each combination of direction ψ _k and distance r at m-1) and a different distance r from the distance sensor;
For each vector A (r, ψ _k ), a two-dimensional environment map composed of a plurality of pixels is set in a direction opposite to the direction indicated by the vector A (r, ψ _k ). , ψ _k ) to create a reference map (r, ψ _k ) by translating by the distance indicated by
Sensing data measured by the reference sensor (r, ψ _k ) and the distance sensor, and n directions φ _i (i = 0, 1, ··· n-1), and the distance from the distance sensor in each direction phi _i to the object d _{i (i} = 0,1, sensing indicating the ··· n-1) data (d _i, phi _i ) And a position and orientation estimation method in a position and orientation estimation device for estimating the position and orientation of the device in which the distance sensor is mounted in the environment map,
The position / orientation estimation apparatus includes:
A reference map creating step for creating a reference map (r, ψ _k ) (k = 0, 1, ... m-1) from the environmental map;
Referring to reference maps created in the reference map creation step (r, ψ _k), for each same distance r, the reference map (r, ψ _k) - by reference map _{(r, ψ k-1)} , next The difference between the reference maps in the matching direction is calculated for each pixel, and the difference image (r, ψ _k ) (k = 1, 2,...) That collects pixels having pixel values other than 0 in the calculated difference for each pixel. A difference image creation step for creating m-1),
For each of n pieces of sensing data (d _i , φ _i ) (i = 0, 1,... N-1) measured by the distance sensor, the sensing data (d _i , φ _i ) is indicated. A reference map (d _i , φ _i ) corresponding to the same distance and direction as the distance d _i and the direction φ _i is extracted from the reference map, and the extracted n reference maps (d _i , φ _i ) pixels by adding the value for each pixel, and an adding pixel values, the evaluation image to the angle of theta ₀ and direction as the main direction of the environmental map of the distance sensor is mounted device (theta ₀ ) a first evaluation image creation step,
A first evaluation value calculating step of calculating a pixel value at a position of (x, y) in the evaluation image (θ ₀ ) as an evaluation value in (x, y, θ ₀ );
For each of the n sensing data (d _i , φ _i −θ _j ) measured by the distance sensor from j = 1 to j = m−1, the sensing data (d _i , φ _i −θ _j ) The difference image (d _i , φ _i -θ _j ) corresponding to the same distance and direction as the distance d _i and direction φ _i -θ _j shown in FIG. By adding the pixel values of (d _i , φ _i −θ _j ) for each pixel, the orientation of the device in which the distance sensor is mounted and the main direction of the environment map, which are configured from the added pixel values, An integrated difference image (θ _j ) with an angle formed by θ _j is created, and the evaluation image (θ _j-1 ) and the integrated difference image (θ _j ) are added for each pixel to obtain an evaluation image ( The second evaluation value calculation step of creating θ _j ) and calculating the pixel value at the position (x, y) in the evaluation image (θ _j ) as the evaluation value in (x, y, θ _j ) When,
Among the evaluation values calculated in the first evaluation value calculation step and the evaluation values calculated in the second evaluation value calculation step, (x, y, θ) corresponding to the maximum evaluation value is specified. A position and orientation estimation step for estimating an orientation and a position of a device in which the distance sensor is mounted at a position (x, y) in the environment map, with an angle formed with the main direction of the environment map being a direction θ; A position / orientation estimation method comprising: Different angle between the main direction of the distance sensor, the order in [psi ₀ angle is smaller with the main _{direction, ψ 1, ··· ψ m-} 1 to m-number of directions ψ _{k (k} = 0,1, · A vector A (r, ψ _k ) is defined for each combination of direction ψ _k and distance r at m-1) and a different distance r from the distance sensor;
For each vector A (r, ψ _k ), a two-dimensional environment map composed of a plurality of pixels is set in a direction opposite to the direction indicated by the vector A (r, ψ _k ). , ψ _k ) to create a reference map (r, ψ _k ) by translating by the distance indicated by
Sensing data measured by the reference sensor (r, ψ _k ) and the distance sensor, and n directions φ _i (i = 0, 1, ··· n-1), and the distance from the distance sensor in each direction phi _i to the object d _{i (i} = 0,1, sensing indicating the ··· n-1) data (d _i, phi _i ) To estimate the position and orientation of the device in which the distance sensor is mounted in the environmental map,
A reference map creating means for creating a reference map (r, ψ _k ) (k = 0, 1, ... m-1) from the environmental map;
With reference to the reference map (r, ψ _k ) created by the reference map creating means, the reference map (r, ψ _k ) −reference map (r, ψ _k−1 ) for the same distance r, A difference between reference maps in adjacent directions is calculated for each pixel, and a difference image (r, ψ _k ) (k = 1, 2,...) That collects pixels having pixel values other than 0 in the calculated difference for each pixel. ..M-1) difference image creation means,
For each of n pieces of sensing data (d _i , φ _i ) (i = 0, 1,... N-1) measured by the distance sensor, the sensing data (d _i , φ _i ) is indicated. A reference map (d _i , φ _i ) corresponding to the same distance and direction as the distance d _i and the direction φ _i is extracted from the reference map, and the extracted n reference maps (d _i , φ _i ) pixels by adding the value for each pixel, and an adding pixel values, the evaluation image to the angle of theta ₀ and direction as the main direction of the environmental map of the distance sensor is mounted device (theta ₀ ) a first evaluation image creating means for creating
A first evaluation value calculating means for calculating a pixel value at a position of (x, y) in the evaluation image (θ ₀ ) as an evaluation value in (x, y, θ ₀ );
For each of the n sensing data (d _i , φ _i −θ _j ) measured by the distance sensor from j = 1 to j = m−1, the sensing data (d _i , φ _i −θ _j ) The difference image (d _i , φ _i -θ _j ) corresponding to the same distance and direction as the distance d _i and direction φ _i -θ _j shown in FIG. By adding the pixel values of (d _i , φ _i −θ _j ) for each pixel, the orientation of the device in which the distance sensor is mounted and the main direction of the environment map, which are configured from the added pixel values, Integrated difference image creating means for creating an integrated difference image (θ _j ) with an angle formed by θ _j ,
From j = 1 to j = m−1, an evaluation image (θ _j ) is created by adding the evaluation image (θ _j-1 ) and the integrated difference image (θ _j ) for each pixel. Evaluation image creation means,
A second pixel value is calculated as an evaluation value at (x, y, θ _j ) from j = 1 to j = m−1 at the position (x, y) in the evaluation image (θ _j ). An evaluation value calculating means;
Among the evaluation values calculated by the first evaluation value calculation means and the evaluation values calculated by the second evaluation value calculation means, (x, y, θ) corresponding to the maximum evaluation value is A position and orientation estimation that identifies and estimates the orientation and position of the device in which the distance sensor is mounted at the position (x, y) in the environment map with an angle of θ with the main direction of the environment map A position and orientation estimation device.

Images