JP2014174275A - Geographical map preparation device, geographical map preparation program, and geographical map preparation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a new geographical map preparation device, program and method which can create a map simply, in order to solve the problem with the background technology in which two maps need to be created and thus the process load for creation is high and it is troublesome to integrate the information of two maps.SOLUTION: A geographical map preparation system 10 includes a movable body 12 which moves throughout the environment. Distance data is detected by distance sensors 22a, 22b provided in the movable body, and the number of rotations of the left and right wheels 16a, 16b each of the movable body is also detected. A computer (30) calculates a position and an azimuth of the movable body on the basis of rotation rate data, and converts distance data into position data corresponding to a position of a measured body. This position data is subjected to clustering by a predetermined rule, and clusters (position data) other than a static body such as a wall and a floor are eliminated on the basis of a feature quantity of each cluster. Consequently, the position of the movable body is estimated using position data on the static body, and the geographical map on the environment is prepared. Thus, a geographical map can be simply prepared by eliminating data unnecessary for preparing the geographical map from position data based on distance data.

Description

  The present invention relates to a map creation device, a map creation program, and a map creation method, and more particularly, to a map creation device, a map creation program, and a map creation method for creating a map for a certain environment using a moving body equipped with a distance sensor. .

  An example of background art of the present invention is disclosed in Non-Patent Document 1. In this non-patent document 1, an occupancy grid map including information on a static part of an environment that does not move, such as a wall, and an occupancy grid map including information on a dynamic object that has moved even once are created and updated. By integrating map information, a map is created and the position of the mobile robot is estimated using SLAM (Simultaneous Localization and Mapping) technology.

Denis Wolf, et al .: “Online simultaneous localization and mapping in dynamic environments”, In Proc. Of the Intl. Conf. On Robotics and Automation (ICRA), 2004.

  However, in this background art, it is necessary to create two maps, and the load of creation processing is large. It also takes time to integrate the information in the two maps.

  Therefore, the main object of the present invention is to provide a novel map creation device, map creation program, and map creation method.

  Another object of the present invention is to provide a map creation device, a map creation program, and a map creation method capable of easily creating a map.

  1st invention is the map creation apparatus which creates the map about the environment where the said mobile body moved while estimating the position of the said mobile body based on the detection result of the distance sensor installed in the mobile body, Data conversion means for converting the distance data detected by the sensor into position data based on the position and orientation of the moving body when measuring the distance data, and a plurality of position data converted by the data conversion means according to a predetermined rule Clustering means for clustering into clusters, condition judging means for judging whether each of a plurality of clusters clustered by the clustering means satisfies a predetermined condition, and condition judging means for judging that the predetermined condition is not satisfied It is a map creation apparatus provided with the erasure | elimination means which erase | eliminates a cluster.

  In the first invention, the map creation device estimates the position of the mobile body based on the detection result of the distance sensor installed on the mobile body and creates a map of the environment in which the mobile body has moved. The data conversion means converts the distance data detected by the distance sensor into position data based on the position and orientation of the moving body when the distance data is measured. The clustering means clusters the position data converted by the data conversion means into a plurality of clusters according to a predetermined rule. The condition determining unit determines whether each of the plurality of clusters clustered by the clustering unit satisfies a predetermined condition. For example, the predetermined condition is a condition regarding the feature of the cluster, and more specifically, for determining whether the object is a dynamic object such as a human or a static object such as a wall or a column. It is a condition. The erasing unit erases the cluster that is determined not to satisfy the predetermined condition by the condition determining unit. That is, unnecessary position data is deleted.

  According to the first invention, unnecessary position data is erased, so that a map can be created based only on necessary data. Therefore, a map can be easily created.

  A second invention is dependent on the first invention, and the predetermined rule is that a distance between points indicated by position data is equal to or less than a predetermined distance or less than a predetermined distance.

  In the second invention, the predetermined rule is that the distance between the points indicated by the position data is equal to or less than the predetermined distance or less than the predetermined distance, and the nearby points are classified into the same cluster.

  According to the second invention, since only the distance between points is detected, clustering can be easily performed.

  A third invention is dependent on the first or second invention, and the predetermined condition includes that the number of points indicated by the position data included in the cluster is equal to or less than a predetermined number or less than a predetermined number.

  In the third invention, the predetermined condition includes that the number of points indicated by the position data included in the cluster is equal to or less than the predetermined number or less than the predetermined number. Erased. For example, when the number of points included in the cluster is small, it cannot be determined whether the object is a dynamic object or a static object, and thus the cluster is deleted.

  According to the third aspect of the invention, it is possible to delete an unnecessary cluster that cannot be determined whether it is a dynamic object or a static object.

  A fourth invention is dependent on the first to third inventions, and further comprises an extracting means for extracting each feature of the plurality of clusters clustered by the clustering means, and the condition judging means is extracted by the extracting means It is determined whether the feature satisfies a predetermined condition.

  In the fourth invention, the extraction means extracts each feature of the plurality of clusters. The condition determining means determines whether or not the feature of each cluster satisfies a predetermined condition.

  According to the fourth invention, it is possible to determine whether or not a predetermined condition is satisfied by a feature other than the number of points included in the cluster.

  A fifth invention is according to the fourth invention, and the feature extracted by the extracting means is a feature quantity regarding a variance of a plurality of points indicated by position data included in the cluster.

  In the fifth invention, the feature of the cluster is a feature amount regarding the dispersion of a plurality of points indicated by the position data included in the cluster, for example, a feature related to the shape of the cluster.

  According to the fifth aspect, it is possible to determine whether the object is a dynamic object or a static object depending on whether or not the feature related to the shape of the cluster satisfies a predetermined condition.

  A sixth invention is according to the fifth invention, wherein the feature amount is an eigenvalue of a variance matrix of a plurality of points indicated by position data included in the cluster, and the predetermined condition is that the maximum eigenvalue is the first value. It further includes being larger than the predetermined value, or having the second largest eigenvalue less than the second predetermined value smaller than the first predetermined value.

  In the sixth invention, the feature amount is an eigenvalue for a variance matrix of a plurality of points indicated by position data included in the cluster. For example, the feature amount is a principal component for the variance of a plurality of points included in the cluster, and is represented by an eigenvalue of the variance matrix. For example, the predetermined condition further includes that the maximum eigenvalue is larger than the first predetermined value, or that the second largest eigenvalue is less than the second predetermined value smaller than the first predetermined value. Therefore, when the maximum eigenvalue is less than or equal to the first predetermined value, or when the second largest eigenvalue is greater than or equal to the second predetermined value, it is determined that the predetermined condition is not satisfied, and the cluster having the eigenvalue Is erased.

  According to the sixth aspect, unnecessary data can be erased based on the eigenvalues of the dispersion matrix of a plurality of points included in the cluster.

  A seventh aspect of the invention is a computer map creation program for estimating a position of a moving body based on a detection result of a distance sensor installed on the moving body and creating a map of an environment in which the moving body has moved. A data conversion step for converting the distance data detected by the distance sensor into position data based on the position and orientation of the moving body at the time of measurement; and the position data converted in the data conversion step A clustering step of clustering into a plurality of clusters according to the rules of the above, a condition determining step for determining whether each of the plurality of clusters clustered in the clustering step satisfies a predetermined condition, and a predetermined condition not being satisfied in the condition determining step Erase the determined cluster To perform the erase step that is a map creation program.

  An eighth invention is a map creation method for a computer that estimates a position of the mobile body based on a detection result of a distance sensor installed on the mobile body and creates a map of an environment in which the mobile body has moved. The computer (a) converts the distance data detected by the distance sensor into position data based on the position and orientation of the moving body at the time of measurement, and (b) converted the data in step (a). The position data is clustered into a plurality of clusters according to a predetermined rule, (c) it is determined whether each of the plurality of clusters clustered in step (b) satisfies a predetermined condition, and (d) in step (c) This is a map creation method for deleting clusters that are determined not to satisfy a predetermined condition.

  In the seventh and eighth inventions, as in the first invention, a map can be easily created.

  According to the present invention, the positions measured by the distance sensor installed on the moving body are clustered into a plurality of clusters according to a predetermined rule, and it is determined whether or not each cluster satisfies a predetermined condition. Since a cluster other than an object is deleted, a map can be easily created.

  The above object, other objects, features and advantages of the present invention will become more apparent from the following detailed description of embodiments with reference to the drawings.

FIG. 1 is an illustrative view showing one example of an external configuration of a map creation system of the present invention. FIG. 2 is a block diagram showing an example of an electrical configuration of the map creation system shown in FIG. FIG. 3 is an illustrative view for explaining a detection range of the distance sensor shown in FIGS. 1 and 2. FIG. 4 is an illustrative view for explaining parameters for calculating the position and orientation of the moving body. FIG. 5 is an illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 6 is another illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 7 is another illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 8 is still another illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 9 is an illustrative view for explaining a specific case of determining a static object. FIG. 10 is an illustrative view for explaining a state in which clusters other than those determined as static objects are deleted. FIG. 11 is an illustrative view showing one example of a created map. FIG. 12 is an illustrative view showing an example in which points on which a distance is measured for a dynamic object are further displayed on a created map. FIG. 13 is an illustrative view showing one example of a memory map of the RAM shown in FIG. FIG. 14 is a flowchart showing the overall processing of the CPU shown in FIG. FIG. 15 is a flowchart showing a part of the unnecessary data erasing process of the CPU shown in FIG. FIG. 16 is another part of the unnecessary data erasing process of the CPU shown in FIG. 1, and is a flowchart subsequent to FIG.

  Referring to FIG. 1 (A), a mapping system 10 of this embodiment includes a moving body 12 such as an electric wheelchair, and the moving body 12 has left and right front wheels (casters) 14a, 14b and Left and right rear wheels 16a and 16b are provided. An operation lever 18 is provided on the upper part of the left frame of the moving body 12. Further, a distance sensor 22a is attached to the left frame of the moving body 12 on the left side of the step 20a where the left foot is placed. However, the distance sensor 22a is a right frame of the moving body 12, and may be attached to the right side of the step 20b where the right foot is placed. Further, as shown in FIG. 1B, which is a schematic view of the moving body 12 as viewed from above, a distance sensor 22 b is provided behind the moving body 12.

  In this embodiment, the distance sensors 22a and 22b are attached to the front and rear of the moving body 12, but may be attached to the left and right of the moving body 12. The reason why the two distance sensors 22a and 22b are provided on the front and rear or the left and right sides of the moving body 12 is to detect (measure) the distance around the moving body 12 over the entire circumference (360 °). However, this is merely an example, and there may be one distance sensor or three or more distance sensors.

  In this embodiment, the distance sensors 22a and 22b detect not only fixed objects (static objects) such as walls and pillars in a certain environment but also moving objects (dynamic objects) such as humans ( Discriminate). For this reason, as will be described later, the distance sensors 22a and 22b are installed such that their detection positions are at a predetermined height from the ground or floor. In this embodiment, the predetermined height is 11 cm. This is to detect a human leg (around the ankle).

  Referring back to FIG. 1A, a box 24 is provided below the seat of the moving body 12 and between the left rear wheel 16a and the right rear wheel 16b. A computer 30, an input / output interface (hereinafter simply referred to as “interface”) 32, motor drivers 34a and 34b, motors 36a and 36b, and encoders 38a and 38b, which will be described later, are provided (see FIG. 2). However, the computer 30 and the interface 32 may be provided outside the box 24.

  FIG. 2 is a block diagram showing an electrical configuration of the map creation system 10 shown in FIGS. 1 (A) and 1 (B). As shown in FIG. 2, the map creation system 10 includes a computer 30 that functions as a map creation device and a position estimation device for the moving body 12. The computer 30 is a computer such as a general-purpose personal computer or workstation, and includes components such as a CPU 30a, a RAM 30b, and an HDD 30c.

  Further, the operation lever 18 and the distance sensors 22a and 22b described above are connected to the computer 30 and an interface 32 is connected thereto. A motor 36a is connected to the interface 32 via a motor driver 34a, and a motor 36b is connected via a motor driver 34b. The interface 32 is connected to encoders 38a and 38b.

  Although not shown, the rotation shaft of the motor 36a and the rotation shaft of the left rear wheel 16a are connected using a gear, and the rotation shaft of the motor 36b and the rotation shaft of the right rear wheel 16b are connected using a gear. .

  The computer 30 governs overall control of the map creation system 10 of this embodiment. In this embodiment, the CPU 30a controls the driving of the motors 36a and 36b in accordance with the operation input from the operation lever 18, that is, controls the movement of the moving body 12, and the distance sensors 22a and 22b and the encoders 38a and 38b. The distance data and the rotation speed data detected in step 1 are acquired and stored. Further, the computer 30 creates a map or estimates (determines) the position of the moving body 12 based on the detected data using SLAM technology.

  The operation lever 18 is operated by a user and gives a signal (operation input) corresponding to the operation to the computer 30. For example, operations such as forward, backward, stop, left turn, right turn, and turn can be performed.

  However, an operation input can also be given to the computer 30 by remote operation using a remote controller (not shown).

  The distance sensors 22a and 22b are, for example, general-purpose laser range finders (LRF), and measure the distance to the object from the time it takes to irradiate the laser and reflect it back to the object. For example, a laser beam emitted from a transmitter (not shown) is reflected by a rotating mirror (not shown), and scanned in a fan shape at a certain angle (for example, 0.25 degrees). In this embodiment, the period for scanning the entire detection range (see FIG. 3) is 25 msec.

  FIG. 3 is an illustrative view for explaining detection ranges (measurement ranges) of the distance sensors 22a and 22b. The measurement ranges of the distance sensors 22a and 22b in this embodiment are indicated by a fan shape having a radius R (R≈10 to 15 m). The angle of the fan is 135 ° on each of the left side and the right side with respect to the front direction. That is, the distance can be detected for a range of 270 ° centered on the front.

  Returning to FIG. 2, the motor drivers 34 a and 34 b drive the motors 36 a and 36 b based on an instruction from the computer 30. The encoder 38 a detects the rotational speed (rps) of the motor 36 a and inputs data about the rotational speed (rotational speed data) to the computer 30 via the interface 32. Similarly, the encoder 38b detects the rotational speed of the motor 36b, and inputs rotational speed data regarding the rotational speed to the computer 30 via the interface.

  The computer 30 acquires distance data detected by the distance sensors 22a and 22b and rotation speed data detected by the encoders 38a and 38b, and stores them in the RAM 30b (or the HDD 30c). However, the distance data detected by the distance sensor 22a and the distance data detected by the distance sensor 22b are distinguished, and the distance data detected at the same time point and the rotation speed data are associated with each other.

  In the map creation system 10 having such a configuration, a map is created and the position of the moving body 12 is estimated using the SLAM technology as described above. However, prior to the application of SLAM, the distance data is converted into position data based on the rotational speed data. Specifically, the distance data measured by the distance sensors 22a and 22b mounted on the moving body 12 are two-dimensional coordinates based on the position and orientation of the moving body 12 when the distance data is detected. It is converted into position (point) data of the system (moving body coordinate system). At this time, the attachment positions of the two distance sensors 22a and 22b are considered. That is, the displacement between the position of the moving body 12 and the attachment positions of the distance sensors 22a and 22b is adjusted. Of course, the angle in the measurement range of the distance sensors 22a and 22b when the distance data is detected is also taken into consideration. Furthermore, in this embodiment, position data in the moving body coordinate system is converted into position data in a two-dimensional world coordinate system. That is, in the moving body coordinate system, since the position and orientation when the measurement of the distance data about the environment (movement of the moving body 12) is started as a reference, it is converted to the world coordinate system for the real space. It is.

  In this embodiment, SLAM using a particle filter is applied. In brief, in SLAM, since the position of the moving body 12 is estimated and the map is created at the same time, the position of the moving body 12 is estimated using the previously observed landmarks. The process of growing (creating) the map by arranging new landmarks according to the position is repeatedly performed.

In SLAM, a Bayes filter is used. Therefore, in SLAM, as described above, based on the position data z _{1: t obtained} by converting the distance data detected by the distance sensors 22a and 22b into the world coordinate system and the operation a _{1: t} of the moving body 12, the moving body It is formulated as an estimation problem of the simultaneous probability density of 12 trajectories r _{1: t} and map m. However, the operation a of the moving body 12 is [v, ω] ^T.

Here, as described above, in this embodiment, the measured distance data is converted into position data of the moving object coordinate system based on the position and orientation of the moving object 12. Here, the position and orientation of the moving body 12 are obtained based on the measured values obtained by odometry. Specifically, as shown in FIG. 4, the position and orientation of the moving body 12 are calculated using the movement speeds V _L and V _R and the wheel-to-wheel distance D of the left and right wheels (16a and 16b) [ v, ω] ^T is obtained by tracking.

Specifically, the motion [v, ω] ^T of the moving body 12 is calculated according to Equation 1. However, v is the speed of the moving body 12, and ω is the center of the axle of the wheels 16a and 16b (see FIG. 4), and is the angular speed about the axis perpendicular to the axle.

[Equation 1]

However, the moving speeds V _L and V _R of the left and right wheels (16a, 16b) can be calculated from the rotational speeds of the left and right wheels (16a, 16b), and the wheel interval D can be known in advance. However, the moving velocity V _L corresponding to the rotational speed, is prepared a table of V _R, without computing, it may acquire the moving speed V _L, V _R by referring to the table.

Then, by integrating the calculated operation [v, ω] ^T , the previously calculated movement distance d and rotation angle θ from the position and orientation of the moving body 12 can be obtained. Accordingly, the moving distance d and the rotation angle θ calculated according to the time series are cumulatively added to the initial position and the initial orientation (angle θ = 0) of the moving body 12, thereby moving the moving body every time. 12 positions and orientations can be known.

Regarding the above problem of estimating the joint probability density of the trajectory r _{1: t} of the moving object 12 and the map m, see “S. Thrun, W. Burgard and D. Fox: Probabilistic Robotics. MIT Press, 2005.” Translated by Ryuichi Ueda: Stochastic Robotics, Mainichi Communications, 2007. "(Reference 1).

The state vector locus r ₁ of the moving body _12: set m ₁ of _t as a mark, ..., consisting of m _n. For this reason, it is difficult to apply the particle filter as it is. Therefore, by limiting the sample space of the particles to the trajectory of the moving body 12 by an operation called Rao-Blackwellization, the dimension of the particles is reduced, and the distribution of the mark positions is calculated analytically for each particle. "K. Murphy and S. Russell: 'Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks,' A. Doucet ed .: Sequential Monte Carlo Methods in Practice. Springer, 2001." (Reference 2). This is called Rao-Blackwellised Particle Filter (RBPF) SLAM. As a practical implementation, there is FastSLAM (Reference 1).

In this FastSLAM, the simultaneous probability density of the trajectory r _{1: t} of the moving object 12 and the map m is decomposed as shown in Equation 2, and the trajectory distribution p (r _{1: t} , m | z _{1: t} , a of the moving object is obtained. _{1: t} ) is calculated by the particle filter, and the distribution p (m _j | r _{1: t} , z _{1: t} ) of the mark position is calculated by the Kalman filter.

[Equation 2]

Further, the state (state vector) of each particle i at time t is predicted at time t−1 using control data (here, rotation speed data of wheels 16a and 16b). Next, the similarity (likelihood) between the predicted state and the actual state based on the measured value is calculated, and the importance w ⁽ⁱ⁾ is calculated based on this similarity (resampling). For example, the total number of particles i is fixed, the particles i having a low similarity are deleted, and the particles i having a high similarity are split. Further, the importance w ⁽ⁱ⁾ of each particle i is normalized by 1.

In this embodiment, SLAM using a particle filter is applied, but SLAM using an extended Kalman filter (EKF-SLAM) can also be applied. In the EKF-SLAM, it represents joint probability density of the position r _t and the map m of the moving body 12 in a normal distribution, by using a Bayesian estimation and Markov assumption, constitute a recurrence formula as shown in Formula 3.

[Equation 3]

Here, if u _t = (r _t , m ₁ ,..., M _n ), the desired joint probability is p (u _t | z _{1: t} , a _{1: t} ), and an extended Kalman filter can be implemented. . However, the motion model p (r _t | r _t−1 , a _t ) is calculated for the position r _t, while the measurement model is calculated for u _t .

  However, such a map creation system 10 is detected by the distance sensors 22a and 22b in order to create a map of an environment in which a dynamic object such as a human, a shopping cart, or a cart exists such as a shopping mall or an event venue. Among the position data obtained by coordinate conversion of the distance data (position data in the world coordinate system), it is necessary to delete the position data for a dynamic object such as a human.

  Thus, in this embodiment, dynamic objects other than static objects (environments) such as walls and pillars are discriminated from position data in the world coordinate system, and unnecessary position data for creating a map of the environment is obtained. It is supposed to be erased. Hereinafter, a case where position data about a human or a dynamic object that seems to be a human is deleted will be described.

  For example, in FIG. 5, there are two people in front of the wall. In such a situation, the moving body 12 faces the wall or the person, and the distance is detected by the distance sensor 22 a provided on the moving body 12. It shows how to do. However, in FIG. 5, the wall, the person, and the moving body 12 are viewed from above. Further, here, for the sake of simplicity, the measurement range of the distance sensor 22a is a range of about 90 ° centered on the front surface, and the measurement range is shown as being scanned by about 3 °. Furthermore, in FIG. 5, the distance to be detected (measured) is indicated by a dotted line. Further, as described above, the distance sensor 22a is installed at a predetermined height from the floor or the ground. Therefore, for humans, the distance from the leg (around the ankle) is measured.

  A method for erasing unnecessary position data in the case shown in FIG. 5 will be described. Briefly, the positions (points) corresponding to the position data are clustered according to a predetermined rule, a person is discriminated from the characteristics of the clustered point group (cluster), and the cluster (position (position ( Point) delete a set of data). However, when the number of points included in the cluster is less than a predetermined number NP (5 in this embodiment), it is difficult to determine whether the object is a dynamic object or a static object. Such clusters are also erased. The number of points included in this cluster is also one of the features of the cluster.

  In clustering, positions (points) whose distance is less than a predetermined value TH (10 cm in this embodiment) are classified into the same cluster. Specifically, a point having the shortest (short) distance to the point of interest is detected, and it is determined whether the distance is less than a predetermined value TH. If the distance is less than the predetermined value TH, the detected point is classified into the same cluster as the point of interest. On the other hand, when the distance is equal to or greater than the predetermined value TH, the detected point is classified into a new cluster different from the point of interest. Then, the clustering as described above is performed with the point classified as a new cluster as a point of interest. In this way, clustering is performed for all points.

  FIG. 6 shows points (positions) whose distances are measured by the distance sensor 22a of the moving body 12 in FIG. However, in FIG. 6, a wall and a person are omitted for easy understanding of the point (position) from which the distance is measured. Such points are clustered by the method described above. However, as described above, actually, clustering is performed on the position data coordinate-converted to the world coordinate system.

  FIG. 7 shows clusters generated when clustering is performed on each point (position) shown in FIG. 6 by the method described above. As can be seen from FIG. 7, for example, seven clusters are generated. In FIG. 7 (FIGS. 8 and 9 are also the same), dots are enclosed to indicate that they are classified into the same cluster.

When clustering is completed, the features for each cluster are extracted (calculated). In this embodiment, the feature of the cluster is a principal component for the dispersion of a plurality of points included in the cluster, and the eigenvalues λ ₁ and λ ₂ for the dispersion matrix. However, λ ₁ is the largest eigenvalue among eigenvalues for the dispersion matrix, and λ ₂ is the second (second) largest eigenvalue after eigenvalue λ ₁ . In FIG. 8, for the sake of easy understanding, the features of clusters (principal components of dispersion) are indicated by solid lines, and points included in the clusters are omitted.

  As described above, since it is difficult to discriminate whether a cluster is a dynamic object or a static object with respect to a cluster in which the number of points included in the cluster is less than a predetermined number NP, it is not necessary to calculate an eigenvalue. Is done. Therefore, as shown in FIGS. 7 and 8, the cluster 4 having four points is erased. In FIG. 8 (the same applies to FIG. 10), the fact that the cluster 4 has been erased is expressed by describing a line surrounding a point included in the cluster 4 with a dotted line.

When calculating the eigenvalues λ ₁ and λ ₂ , first, in each cluster, the centroid (or average) (μ _x , μ _y ) for a plurality of points included in the cluster is calculated according to Equation 4. It is assumed that n points are included in the cluster.

[Equation 4]

The cluster dispersion matrix Σ for the case where the calculated center of gravity (μ _x , μ _y ) is used as a reference is expressed by Equation 5, and its eigenvalue is calculated according to Equation 6.

[Equation 5]
Σ _ij = E [(X _i −μ _i ) (X _j −μ _j )] = E (X _i X _j ) −E (X _i ) E (X _j )
However, μ _i = E (X _i ), and in this embodiment, i = j = 1, 2 in order to calculate _two eigenvalues λ ₁ and λ ₂ .

[Equation 6]
Σx−λIx = (Σ−λI) x = 0
det (Σ-λI) = 0
However, in this embodiment, since two eigenvalues λ ₁ and λ ₂ are calculated, I is a 2 × 2 unit matrix.

When the eigenvalues λ ₁ and λ ₂ are calculated in this way, the eigenvalues λ ₁ and λ ₂ are used to determine whether the object is a dynamic object or a static object. Specifically, when the eigenvalue λ ₁ is larger than a first threshold value (for example, 65 cm), or when the eigenvalue λ ₂ is smaller than a second threshold value (for example, 3 cm), such eigenvalue λ ₁ , A cluster having λ ₂ is determined as a static object. However, the first threshold value and the second threshold value are numerical values obtained empirically in order to discriminate between a dynamic object such as a human and a static object such as a wall or a column.

Thus, when the eigenvalue λ ₂ is smaller than the second threshold value, the shape of the cluster is long and is considered to be a cluster corresponding to the wall. In addition, when the eigenvalue λ ₁ is larger than the first threshold value, as shown in FIG. 9, it can be considered as a cluster corresponding to a wall or a column of a portion corresponding to the corner of the passage. Therefore, when the eigenvalue λ ₁ is equal to or smaller than the first threshold value or when the eigenvalue λ ₂ is equal to or larger than the second threshold value, an object corresponding to a human leg (around the ankle), that is, a human or an object like a human being is used. A cluster having such eigenvalues λ ₁ and λ ₂ is determined as a dynamic object, and the cluster is deleted.

However, in this embodiment, when both the eigenvalues λ ₁ and λ ₂ are smaller than the second threshold value (for example, 3 cm), it is determined that the object is too small, and the cluster having the eigenvalues λ ₁ and λ ₂ is determined. to erase.

  FIG. 10 determines whether the cluster 1-3, 5-7 shown in FIG. 8 is a static object or a dynamic object based on the respective characteristics, and is determined to be a dynamic object. FIG. 11 is a diagram in which the clusters (clusters 2, 3, 5, and 6 in FIG. 10) are deleted. Accordingly, as shown in FIGS. 5-8 and 10, only clusters for static objects such as walls and pillars are left, clusters for dynamic objects such as humans and dynamic or static objects. Unnecessary clusters (a set of position data) such as clusters that cannot be discriminated are deleted.

  FIG. 11 is an illustrative view showing a part of a map created by using the SLAM technique after deleting unnecessary clusters as described above. However, in FIG. 11 (FIG. 12 is also the same), the movement path of the moving body 12 is indicated by a single solid line, and other areas indicated by small black dots are walls or pillars. Static objects such as

  In addition, FIG. 12 is a map in which a cluster that is determined to be a dynamic object such as a human in the map shown in FIG. 11 is also drawn. In FIG. 12, a region indicated by a plurality of gray dots is a dynamic object such as a human. Since there is no such dynamic object in the map shown in FIG. 11, for example, when the mobile body 12 such as a communication robot provides services such as route guidance, in order to estimate the position of the mobile body 12 itself. It can be used as a map to use.

  FIG. 13 shows an example of the memory map 300 of the RAM 30b built in the computer 30 shown in FIG. As shown in FIG. 13, the RAM 30 b includes a program storage area 302 and a data storage area 304.

  The program storage area 302 stores a main processing program 302a, a data storage program 302b, a data conversion program 302c, an unnecessary data deletion program 302d, a map creation program 302e, and the like.

  The main processing program 302a is a program for processing the main routine of the map creation system 10. The data storage program 302b is a program for acquiring distance data and rotation speed data and storing it in the data storage area 304 of the RAM 30b. However, when the moving body 12 is moved, the distance data and the rotation speed data may be stored in the HDD 30c and read from the HDD 30c to the RAM 30b at the time of creating a map.

  The data conversion program 302c is a program for converting the distance data into the coordinate data of the moving object coordinate system based on the position and orientation of the moving object 12, and further converting the coordinate data into the position data of the world coordinate system. The unnecessary data erasing program 302d is a program for erasing a cluster (a set of position data) determined as a dynamic object and a cluster where a dynamic object or a static object cannot be determined prior to map creation. The map creation program 302e is a program for creating a map by applying SLAM.

  Although illustration is omitted, other programs necessary for map creation are also stored in the program storage area 302.

  The data storage area 304 is provided with a distance data buffer 304a and a rotation speed data buffer 304b. The distance data buffer 304a stores distance data according to the data storage program 302b. Similarly, the rotation speed data buffer 304b stores the rotation speed data in accordance with the data storage program 302b.

  The data storage area 304 stores location data 304c, cluster data 304d, and map data 304e. The position data 304c is position (point) data that has been coordinate-converted into the world coordinate system in accordance with the data conversion program 302c. The cluster data 304d is data for each set of position data (cluster) clustered when deleting unnecessary clusters (set of position data) according to the unnecessary data erasing program 302d. The map data 304e is map data created according to the map creation program 302e.

  Although illustration is omitted, in the data storage area 304, other data necessary for creating a map is stored, and a flag and a timer (counter) necessary for creating a map are provided.

  FIG. 14 is a flowchart showing the overall processing of the CPU 30a shown in FIG. As shown in FIG. 14, when starting the entire process, the CPU 30a drives the distance sensors 22a and 22b in step S1. Then, the CPU 30a starts acquiring and storing distance data from the distance sensors 22a and 22b.

  In the next step S3, movement control of the moving body 12 is started. That is, the CPU 30a executes the movement control process for the moving body 12 in parallel with the overall process. Although detailed description and illustration are omitted, in the movement control process, the CPU 30a controls the motor drivers 34a and 34b according to the operation input input from the operation lever 18 to drive the motors 36a and 36b.

  In the subsequent step S5, the distance data and the rotational speed data are acquired and stored. That is, the CPU 30a acquires distance data from the distance sensors 22a and 22b, acquires rotation speed data from the encoders 38a and 38b, and associates them with each other and stores them in the RAM 30b according to time series.

  Subsequently, in step S7, it is determined whether or not the entire environment range has been moved. Here, it is determined whether or not the movement is finished by the user. Specifically, the CPU 30a determines whether there is no operation input from the operation lever 18 after a predetermined time (for example, 30 seconds) or a movement end instruction is input from the user.

  If “NO” in the step S7, that is, if the movement of the entire range of the environment is not completed, the process returns to the step S3 as it is. On the other hand, if “YES” in the step S7, that is, if the entire range of the environment is moved, the distance data is converted into position data in accordance with the position and orientation of the moving body 12 in a step S9. Here, as described above, the distance data detected by each of the distance sensors 22a and 22b is coordinate-converted into position data of the moving object coordinate system based on the position and orientation of the moving object 12. At this time, as described above, the attachment positions of the two distance sensors 22a and 22b are considered.

  In the subsequent step S11, an unnecessary data erasure process (see FIGS. 15 and 16) described later is executed, and in step S13, a map is created by applying SLAM to the clusters (position data) remaining after the erasure process. In step S15, the map data 304e corresponding to the created map is stored in the RAM 30b, and the entire process is terminated.

  As described above, since the detection cycle of the distance sensors 22a and 22b is 20 msec, the scan time in steps S5 to S7 is set to be shorter than 20 msec.

  FIGS. 15 and 16 are flowcharts showing the unnecessary data erasing process in step S11 shown in FIG. As shown in FIG. 15, when the CPU 30a starts the unnecessary data erasing process, the position data in the moving object coordinate system is converted into position data in the world coordinate system in step S31.

  In the next step S33, a new cluster is generated for the point of interest (position corresponding to the position data). For example, in step S33, position data not yet classified into clusters is arbitrarily selected, and a new cluster is generated. At this time, new cluster data including position data for the point of interest is added. That is, the cluster data 304d is updated by adding new cluster data.

  In a succeeding step S35, it is determined whether or not the distance to the next point is less than a predetermined value TH (10 cm in this embodiment). Here, the CPU 30a determines whether or not the distance from the point closest to the point of interest is less than the predetermined value TH. If “NO” in the step S35, that is, if the distance to the next point is equal to or greater than the predetermined value TH, the process returns to the step S33 in order to generate a new cluster. On the other hand, if “YES” in the step S35, that is, if the distance to the next point is less than the predetermined value TH, the next point is added to the existing cluster in a step S37. That is, the next point is classified into the same cluster as the point of interest.

  In step S39, it is determined whether clustering has been performed for all points. Here, the CPU 30a determines whether or not position data that is not classified into clusters remains. If “NO” in the step S39, that is, if a point where clustering is not performed remains, the process returns to the step S35. Although illustration is omitted, at this time, the point that has been classified into the cluster immediately before is set as the point of interest.

  On the other hand, if “YES” in the step S39, that is, if clustering is performed for all points, it is determined that the clustering is finished, the first cluster is read in a step S41, and in the step S43 illustrated in FIG. It is determined whether or not the number of points included in the cluster is less than a predetermined number NP (5 in this embodiment).

  In step S41, the first cluster is described as being read for convenience of explanation, but an arbitrary cluster is read here. The same applies to step S57 described later, but in this step S57, an arbitrary cluster that has not been determined to be a static object is read. However, the clusters may be read in the order in which the clusters are generated.

If “YES” in the step S43, that is, if the number of points included in the cluster is less than the predetermined number NP, it is determined that it cannot be determined whether the object is a static object or a dynamic object, and the process proceeds to the step S53. . On the other hand, if “NO” in the step S43, that is, if the number of points included in the cluster is equal to or greater than the predetermined number NP, the eigenvalues λ ₁ and λ _{2 of the} cluster are calculated in accordance with the equations 4 to 6 in a step S45. Is calculated.

In the next step S47, it is determined whether both eigenvalues λ ₁ and λ ₂ are less than the second threshold value H _min (3 cm in this embodiment). As described above, it is determined here whether a static object or a dynamic object can be determined. If “YES” in the step S47, that is, if both the eigenvalues λ ₁ and λ ₂ are less than the second threshold H _min , the process proceeds to a step S53 as it is. On the other hand, if "NO" in step S47, the that is, if at least one of the eigenvalues lambda ₁ and lambda ₂ is the second threshold value H _min or more, in step S49, the eigenvalues lambda ₁ is greater than the first threshold value H _{max, Alternatively,} it is determined whether the eigenvalue λ ₂ is less than the second threshold value H _min . As described above, it is determined (discriminated) whether the object is a dynamic object such as a human or a static object such as a wall or a pillar.

If “YES” in the step S49, that is, if the eigenvalue λ ₁ is larger than the first threshold value H _max , or if the eigenvalue λ ₂ is less than the second threshold value H _min , a static object such as a wall or a column is used. In step S51, the cluster is determined as a cluster corresponding to the static object, and the process proceeds to step S55.

On the other hand, if “NO” in the step S49, that is, the eigenvalue λ ₁ is equal to or smaller than the first threshold value H _max , or the eigenvalue λ ₂ is equal to or larger than the second threshold value H _min , a human-like dynamic In step S53, the cluster is erased and the process proceeds to step S55. That is, position data unnecessary for map creation is deleted by executing the processing of step S53.

  In step S55, it is determined whether all clusters have been determined. If “NO” in the step S55, that is, if there is a cluster that has not yet been determined, the next cluster is read in a step S57, and the process returns to the step S43. On the other hand, if “YES” in the step S55, that is, if all the clusters are discriminated, the process returns to the entire processing shown in FIG.

  According to this embodiment, a static object and a dynamic object are discriminated from the measurement result of the distance sensor, and a map is created using the measurement result of the static object. Can be created.

  In this embodiment, since distance data detected by the distance sensor is converted into position data and unnecessary data is deleted, it is not necessary to provide another sensor such as a camera. That is, the cost does not increase and the load such as image processing does not increase.

  In this embodiment, the electric wheelchair is used as the moving body, but it is not necessary to be limited to this. For example, a communication robot “Robovie (registered trademark)” developed by the applicant of the present application and manufactured and sold can be used.

  In this embodiment, the distance data detected by the distance sensor is converted into position data in the moving body coordinate system, and further converted into position data in the world coordinate system. If it is possible to know the position and orientation of the moving object in the real space when the (moving object moving) is started, it is not necessary to convert the position data of the moving object coordinate system to the position data of the world coordinate system.

  Furthermore, in this embodiment, the position data of the moving object coordinate system is converted into the position data of the world coordinate system and then clustered. However, the position data of the moving object coordinate system is converted into the world coordinate system after clustering. You may do it.

  Furthermore, in this embodiment, a case where a human or a human-like object is discriminated as an example of a dynamic object has been described in detail. However, as described above, other dynamic objects such as a shopping cart and a carriage are discriminated. You can also However, when discriminating other dynamic objects such as shopping carts and carts, the number of position data (points) included in the cluster and the conditions that the principal components (eigenvalues of the variance matrix) for the variance satisfy are satisfied. It is necessary to decide about the case.

  In addition, the specific numerical values shown in this embodiment are merely examples, and should not be limited, and can be appropriately changed according to the product to be implemented. Further, in this embodiment, in the step of determining the magnitude using the predetermined value or threshold, the predetermined value or the threshold is not included, but the size is determined including the predetermined value or the threshold. May be.

DESCRIPTION OF SYMBOLS 10 ... Mapping system 12 ... Moving body 16a, 16b ... Wheel 18 ... Operation lever 22a, 22b ... Distance sensor 30 ... Computer 34a, 34b ... Motor driver 36a, 36b ... Motor 38a, 38b ... Encoder

Claims (8)

A map creation device that estimates a position of the mobile body based on a detection result of a distance sensor installed on the mobile body and creates a map of an environment in which the mobile body has moved,
Data conversion means for converting the distance data detected by the distance sensor into position data based on the position and orientation of the moving body when measuring the distance data;
Clustering means for clustering the position data converted by the data conversion means into a plurality of clusters according to a predetermined rule;
Condition determining means for determining whether each of a plurality of clusters clustered by the clustering means satisfies a predetermined condition, and an erasing means for erasing a cluster determined not to satisfy the predetermined condition by the condition determining means A cartography device comprising:   The map creating apparatus according to claim 1, wherein the predetermined rule is that a distance between points indicated by position data is equal to or less than a predetermined distance or less than a predetermined distance.   The map creation device according to claim 1, wherein the predetermined condition includes that the number of points indicated by the position data included in the cluster is equal to or less than a predetermined number or less than a predetermined number. And further comprising an extracting means for extracting each feature of the plurality of clusters clustered by the clustering means,
The map creating apparatus according to claim 1, wherein the condition determining unit determines whether the feature extracted by the extracting unit satisfies the predetermined condition.   The map creation apparatus according to claim 4, wherein the feature extracted by the extraction unit is a feature amount regarding a distribution of a plurality of points indicated by position data included in the cluster. The feature amount is an eigenvalue with respect to a variance matrix of a plurality of points indicated by position data included in the cluster,
The predetermined condition further includes that a maximum value of the eigenvalue is larger than a first predetermined value, or that the second largest eigenvalue is less than a second predetermined value smaller than the first predetermined value. The map creation device according to claim 5. A computer map creation program for estimating a position of the mobile body based on a detection result of a distance sensor installed on the mobile body and creating a map of an environment in which the mobile body has moved,
In the computer,
A data conversion step of converting the distance data detected by the distance sensor into position data based on the position and orientation of the moving body at the time of measurement of the distance data;
A clustering step of clustering the position data converted in the data conversion step into a plurality of clusters according to a predetermined rule;
A condition determining step for determining whether each of the plurality of clusters clustered in the clustering step satisfies a predetermined condition; and an erasing step for deleting a cluster determined not to satisfy the predetermined condition in the condition determining step. Map creation program to be executed. A computer map creation method for estimating a position of the mobile body based on a detection result of a distance sensor installed on the mobile body and creating a map of an environment in which the mobile body has moved,
The computer
(A) converting the distance data detected by the distance sensor into position data based on the position and orientation of the moving body at the time of measurement of the distance data;
(B) clustering the position data converted in step (a) into a plurality of clusters according to a predetermined rule;
(C) determining whether each of the plurality of clusters clustered in step (b) satisfies a predetermined condition; and (d) a cluster determined not to satisfy the predetermined condition in step (c). How to make a map to erase.

Images