JP6156793B2 - POSITION ESTIMATION DEVICE, POSITION ESTIMATION PROGRAM, AND POSITION ESTIMATION METHOD - Google Patents

Description

  The present invention relates to a position estimation apparatus, a position estimation program, and a position estimation method, and more particularly, to a position estimation apparatus, a position estimation program, and a position estimation method that estimate the position of a moving object including a distance sensor, for example.

  An example of background art of the present invention is disclosed in Patent Document 1. In Patent Document 1, an optically special material such as glass is present in a moving area by including not only information on the presence of walls and obstacles but also information indicating the light reflectance attribute of an object in the moving map. Even in this case, self-position estimation is performed while virtually depicting the reflection of the laser beam.

JP2009-223757

  However, since this background art does not consider a dynamic object such as a human moving within the environment, it is difficult to correctly estimate the position of the moving object in an environment where the dynamic object exists.

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

  Another object of the present invention is to provide a position estimation device, a position estimation program, and a position estimation method capable of estimating a position as accurately as possible even when a dynamic object is present.

1st invention is the position estimation apparatus which estimates the position of the said mobile body based on the map data corresponding to the map about a certain environment, and the detection result of the distance sensor installed in the mobile body which moves the said environment The position data calculation means for calculating the position data of the point where the distance is measured from the distance data detected by the distance sensor, and the position data calculated by the position data calculation means is clustered into a plurality of clusters according to a predetermined rule. Clustering means, extraction means for extracting feature quantities for the dispersion of a plurality of points indicated by position data included in each of a plurality of clusters clustered by the clustering means, and feature quantities extracted by the extracting means satisfy a predetermined condition The condition determining means for determining whether or not the condition is satisfied, and the predetermined condition is not satisfied by the condition determining means. It comprises erasing means for erasing the distance data corresponding to the position data included in the cluster is determined is the position estimation device.

In the first invention, the position estimation device estimates the position of the mobile body based on map data corresponding to a map of a certain environment and a detection result of a distance sensor installed in the mobile body moving in the environment. To do. The position data calculation means calculates the position data of the point where the distance is measured from the distance data detected by the distance sensor. For example, the position data may be calculated based on the position of the moving body when the distance is measured, or may be calculated based on the movement start position of the moving body. The clustering means clusters the position data calculated by the position data calculating means into a plurality of clusters according to a predetermined rule. The extraction unit extracts feature amounts for the dispersion of a plurality of points indicated by position data included in each of the plurality of clusters clustered by the clustering unit. For example, the feature amount is a feature related to the shape of the cluster. The condition determining unit determines whether or not the feature amount extracted by the extracting 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 distance data corresponding to the position data included in the cluster that is determined not to satisfy the predetermined condition by the condition determining unit.

According to the first aspect, since unnecessary distance data is deleted, the position can be estimated based only on necessary data. Therefore, the position can be estimated as accurately as possible.
Further, according to the first 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.
Furthermore, according to the first invention, it is possible to determine whether the object is a dynamic object or a static object depending on whether the feature related to the shape of the cluster satisfies a predetermined condition.

  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 according to the inventions of the first or second feature quantity is the eigenvalues of the covariance matrix of a plurality of points indicated by the position data included in the cluster, a predetermined condition, the maximum value of the eigenvalue Is larger than the first predetermined value, or the second largest eigenvalue is less than the second predetermined value smaller than the first predetermined value.

In the third 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 third aspect of the invention, unnecessary data can be erased based on the eigenvalues of the dispersion matrix of a plurality of points indicated by the position data included in the cluster.

According to a fourth aspect of the present invention, there is provided a computer position estimation method for estimating a position of a moving body based on map data corresponding to a map of an environment and a detection result of a distance sensor installed on the moving body moving in the environment. A position data calculation step for calculating position data of a point at which a distance is measured from distance data detected by a distance sensor, and a plurality of position data calculated in the position data calculation step according to a predetermined rule. A clustering step for clustering into clusters, an extraction step for extracting variances of a plurality of points indicated by position data included in each of the plurality of clusters clustered in the clustering step, and the feature amounts extracted in the extraction step are predetermined. Condition judgment to judge whether the condition is met Steps, and in the condition determination step to perform the erasing step of erasing the distance data corresponding to the position data included in the cluster it is determined that not satisfy the predetermined condition, the position estimation program.

According to a fifth aspect of the present invention, there is provided a computer position estimation method for estimating a position of a moving object based on map data corresponding to a map of an environment and a detection result of a distance sensor installed on the moving object moving in the environment. In the method, the computer calculates position data of the point where the distance is measured from the distance data detected by the distance sensor, and (b) a plurality of the position data calculated in step (a) according to a predetermined rule. (C) a step of extracting feature quantities for the dispersion of a plurality of points indicated by position data included in each of the plurality of clusters clustered in step (b), (d) a step (c) And (e) determining whether or not the feature quantity extracted in step (d) satisfies a predetermined condition. Te including the step of erasing the distance data corresponding to the position data included in the cluster is determined to not satisfy the predetermined condition, a position estimating method.

In the fourth and fifth inventions, as in the first invention, the position can be estimated as accurately as possible.

  According to the present invention, position data corresponding to a point whose distance is measured by a distance sensor installed on a moving body is clustered into a plurality of clusters according to a predetermined rule, and it is determined whether each cluster satisfies a predetermined condition. The distance data corresponding to the position data included in the cluster other than the cluster for the static object is deleted, so that the position can be estimated as accurately as possible even in an environment where a dynamic object such as a human exists. it can.

  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 position estimation system of the present invention. FIG. 2 is a block diagram showing an example of an electrical configuration of the position estimation 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 showing an example of a grid map. FIG. 6 is an 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 another illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 9 is still another illustrative view for explaining a method of discriminating between a dynamic object and a static object. FIG. 10 is an illustrative view for explaining a specific case of determining a static object. FIG. 11 is an illustrative view showing an example in which points other than the static object are deleted as a result of discriminating the dynamic object and the static object. FIG. 12 is an illustrative view showing one example of a memory map of the RAM shown in FIG. FIG. 13 is a flowchart showing a part of the entire processing of the CPU shown in FIG. FIG. 14 is another part of the overall processing of the CPU shown in FIG. 2, and is a flowchart subsequent to FIG. FIG. 15 is a flowchart showing a part of the unnecessary data erasing process of the CPU shown in FIG. 16 is another part of the unnecessary data erasing process of the CPU shown in FIG. 2, and is a flowchart subsequent to FIG.

  Referring to FIG. 1A, a position estimation 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 position estimation system 10 shown in FIGS. 1 (A) and 1 (B). As shown in FIG. 2, the position estimation system 10 includes a computer 30 that functions as 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 controls the entire position estimation 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 estimates (determines) the position of the moving body 12 based on the map (map data) about the environment in which the moving body 12 moves (provides services) and the detected data.

  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.

  For example, the mobile body 12 is applied to a robot or the like that can perform communication using at least one of body movements and speech such as gestures, and the robot is an environment to which the robot is applied (such as a shopping mall or an event venue). ) Provide services such as route guidance, customer service, explanation of products and exhibits (guidance). For example, a communication robot “Robovie (registered trademark)” developed by the applicant of the present application and manufactured and sold can be used. The robot moves within the environment to which the robot is applied, and executes a guide according to the moved position (location). Therefore, the robot (moving body 12) needs to know its own position.

  However, the start position and orientation (initial position and initial orientation), passing point, and end point of the robot (moving body 12) are given from the outside, and movement starts from the initial position. For example, the start position, the one or more passing points, and the end point are indicated in the order according to the time series.

Therefore, the position estimation system 10 estimates the position of the moving body 12. In this embodiment, the position of the moving body 12 is estimated using a particle filter. That is, the moving body 12 is tracked. The particle filter is a technique for estimating the current state from the prediction at the previous time and the current observation information using a large number of particles. The particle filter tracks the moving body 12 by repeating prediction, observation, and resampling.
<Forecast>
The position (“state” including direction) of the particle i at the time t is predicted from the particle i at the time t−1. In this embodiment, the state vector of the moving body 12 is x = [x, y, θ] ^T , and is represented by a position (x, y) and a direction (θ). The particle filter includes n particles (400 particles in this embodiment), and the state vector of the ⁱ -th particle is represented by x ⁱ = [x ⁱ , y ⁱ , θ ⁱ ] ^T. In this embodiment, the state vector x ⁱ _t of the particle i at time t is calculated according to Equation 1.

[Equation 1]
x ⁱ _t = f (x ⁱ _t−1 , u _t−1 , Δt)
However, u shown in Equation 1 is the motion information u = [v, ω] ^T of the moving body 12 and is calculated according to Equation 2. Here, 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 around the axis perpendicular to the axle. V _L and V _R are the moving speeds of the left and right wheels (16a, 16b) of the moving body 12, and D is the distance between the wheels (16a, 16b) of the moving body 12.

[Equation 2]

The moving speeds V _L and V _R of the left and right wheels (16a, 16b) can be calculated from the rotation 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 motion u = [v, ω] ^T , the previously calculated position (x _t−1 , y _t−1 ) of the moving body 12 and the moving distance d from the direction θ _t−1. And the rotation angle α can be obtained. However, the initial position (x ₀ , y ₀ ) and the initial orientation θ ₀ are given from the outside. Therefore, the above Equation 1 can be expressed as Equation 3.

[Equation 3]
x ⁱ _t = x ⁱ _t−1 + v · cos (θ _t−1 ) · Δt
y ⁱ _t = y ⁱ _t−1 + v · sin (θ _t−1 ) · Δt
θ ⁱ _t = θ ⁱ _t-1 + ω · Δt
Thus, the state x ⁱ _t of each particle i at state x ⁱ _t-1 from the time t of each particle i at the previous time t-1 is predicted.
<Observation>
The likelihood and importance (weight) w ⁱ _t of each particle i at the predicted position are calculated. In this embodiment, the likelihood is calculated using map data for a grid map as shown in FIG. However, FIG. 5 shows a grid map for a part of the environment in which the moving body 12 moves. As can be seen with reference to FIG. 5, the grid map is generated by drawing a plurality of grid lines at equal intervals in the vertical and horizontal directions. Although detailed description is omitted, the grid map is a two-dimensional map when the environment (real space) in which the moving body 12 moves is viewed from above. That is, the grid map is created in the world coordinate system.

  For example, in the grid map, the interval between the grid lines corresponds to a predetermined length (5 to 10 cm) in the real space. In addition, information indicating that the object is a static object is stored in correspondence with a grid on which a portion corresponding to a static object such as a wall or a column (a surface portion on which a distance can be measured by the distance sensors 22a and 22b) is drawn. ing. In FIG. 5, black is added to the grid in which information indicating that the object is a static object is stored.

On the grid map in the state x ⁱ _t of each particle i at time t, which is estimated as described above measurement results (simulation results) is equal to the actual distance sensors 22a, 22b of the measurement result (measurement value z _t) The probability (likelihood) is obtained according to Equation 4.

[Equation 4]
w ⁱ _t = (z ⁱ _t | x ⁱ _t )
z ⁱ _t = hc / Total
However, in this embodiment, the measured value z _t is the ratio of the number of lasers hc that actually measured the distance out of the total number of lasers scanned in one cycle Total (1080 in this embodiment). However, the measured value z _t is obtained for each of the distance sensors 22a and 22b, and the likelihood is also obtained for each of the distance sensors 22a and 22b.

Then, as shown in Equation 5, the weight w ⁱ _t of each particle i is calculated (determined) by normalizing the likelihood of each particle i with the total likelihood of all the particles i. However, in Equation 5, n is the total number of particles i (400 in this embodiment). In Equation 5, “=” means substitution.

[Equation 5]

In this embodiment, the likelihood is determined as the weight w ⁱ _t , but need not be limited to this. May be determined the weight w ⁱ _t in proportion to the magnitude of likelihood, to the weight w ⁱ _t from the highest particle i in the order of likelihood may be determined so as to linearly change, a constant The weight w ⁱ _t may be determined to change stepwise for each threshold value.

However, for each particle i, the position indicated by the state x ⁱ _t on the grid map (x _t, y _t) at a distance sensor 22a, similarly to 22b, left and right about the direction theta _t showing the state x ⁱ _t The number of hits on the grid in which information indicating that the laser is a static object is detected when scanned at 0.25 degrees in the range of 135 degrees, and the ratio of the number of hits in the total number of scans (1080) Is calculated. However, the laser beam reaches the distance R.
<Resampling>
Particle i is selected by restoration extraction with a probability proportional to the weight w ⁱ _t of each particle i obtained by observation, and a set of particles i at time t is obtained. Accordingly, the particles i having a small weight w ⁱ _t (likelihood) (low) disappear without being selected, and the particles i having a large weight w ⁱ _t (likelihood) are selected many times to increase the number of friends ( Split). The resampled particle i is used for the next estimation. However, in this embodiment, the average coordinate of each particle i is estimated as the position (x _t , y _t ) of the moving body 12, and the average direction of each particle i is estimated as the direction (θ _t ) of the moving body 12. The

The position of the moving body 12 is estimated (corrected) in this way by the geometry calculation (Equation 2 and Equation 3) based on the rotational speed data and the initial position (x ₀ , y ₀ ) and the initial orientation (θ _0). This is because an error occurs due to factors such as the occurrence of slipping of the wheels 16a and 16b when the position and orientation of the moving body 12 are obtained cumulatively from the above. Under the experimental environment, a displacement (error) of about 5% occurred every time the moving body 12 moved 30 cm.

  In addition, such a position estimation system 10 is a position of a mobile body 12 that moves (provides a certain service) in an environment where dynamic objects such as humans, shopping carts, and carts exist, such as shopping malls and event venues. Therefore, it is necessary to delete the distance data when a dynamic object such as a human is measured among the distance data detected by the distance sensors 22a and 22b. This is because such dynamic object information is not stored in the map data (grid map).

  Therefore, in this embodiment, based on the distance data, distance data obtained by measuring a dynamic object other than a static object (environment) such as a wall or a column is discriminated, and distance data unnecessary for position estimation is determined. It is supposed to be erased. Hereinafter, a case where distance data for a human or a dynamic object that seems to be a human is deleted will be described.

  For example, in FIG. 6, there are two people in front of a wall. In such a situation, the moving body 12 faces the wall or the person, and the distance is detected by the distance sensor 22a provided on the moving body 12. It shows how to do. However, in FIG. 6, it is the figure which looked at the wall, the person, and the moving body 12 from upper direction. 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 °. Further, in FIG. 6, the detected (measured) distance 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 distance data in the case shown in FIG. 6 will be described. In this embodiment, first, the two-dimensional data is based on the position and orientation of the moving body 12 when the distance data is detected from the distance data measured by the distance sensors 22a and 22b mounted on the moving body 12. The position (point) data of the coordinate system (moving body coordinate system) is calculated. 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. However, in this embodiment, since the position (x _t , y _t ) and the direction θ _t of the moving body 12 in the world coordinate system are estimated (updated) every predetermined time (for example, 30 seconds), the distance data is Based on the estimated position (x _t , y _t ) in the real space and the direction θ _t , it is converted into position data in a two-dimensional world coordinate system.

  The positions (points) corresponding to the converted position data are clustered according to a predetermined rule, humans are discriminated from the characteristics of the clustered points (clusters), and the positions included in the clusters that are discriminated to be humans (Point) The distance data corresponding to the data is deleted. 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. The distance data corresponding to the position data included in such a cluster is also deleted. The number of position data (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. 7 shows the points (positions) whose distances are measured by the distance sensor 22a of the moving body 12 in FIG. However, in FIG. 7, in order to show the point (position) where the distance is measured in an easy-to-understand manner, a wall and a person are omitted. 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. 8 shows clusters generated when clustering is performed for each point (position) shown in FIG. 7 by the method described above. As can be seen from FIG. 8, for example, seven clusters are generated. In FIG. 8 (FIGS. 9 and 10 are 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. 9, 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. 8 and 9, the cluster 4 having four points is erased. In FIG. 9 (the same applies to FIG. 11), 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. However, actually, distance data corresponding to points (position data) included in the cluster is deleted.

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 6. It is assumed that n points are included in the cluster.

[Equation 6]

The cluster dispersion matrix Σ for the case where the calculated centroid (μ _x , μ _y ) is used as a reference is expressed by Equation 7, and its eigenvalue is calculated according to Equation 8.

[Equation 7]
Σ _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 8]
Σ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. Furthermore, the eigenvalues lambda ₁ is is larger than the first threshold value, as shown in FIG. 10, it is considered that the cluster corresponding to the portion of the wall and pillars corresponding to the corners of the passageway. 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 such as a human being is used. A cluster having such eigenvalues λ ₁ and λ ₂ is determined as a dynamic object, and distance data corresponding to position data included in the cluster is deleted.

However, in this embodiment, when both the eigenvalues λ ₁ and λ ₂ are smaller than the second threshold (for example, 3 cm), it is determined that the object is too small, and the cluster having the eigenvalues λ ₁ and λ ₂ is determined. The distance data corresponding to the included position data is deleted.

  FIG. 11 determines whether the cluster 1-3, 5-7 shown in FIG. 9 is a static object or a dynamic object based on the respective characteristics, and is determined to be a dynamic object. FIG. 12 is a diagram in which the clusters (clusters 2, 3, 5, and 6 in FIG. 11) are deleted. Accordingly, as shown in FIGS. 6 to 9 and 11, only the distance data corresponding to the position data included in the cluster for the static object such as a wall or a column is left, and the dynamic object such as a human is left. The distance data corresponding to the position data included in the unnecessary cluster such as the cluster and the cluster that cannot discriminate the dynamic object or the static object is deleted.

  FIG. 12 shows an example of the memory map 300 of the RAM 30b built in the computer 30 shown in FIG. As illustrated in FIG. 12, 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 position data calculation program 302c, an unnecessary data deletion program 302d, a position estimation program 302e, an autonomous navigation program 302f, and the like.

  The main processing program 302 a is a program for processing the main routine of the position estimation 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.

  The position data calculation program 302c calculates position data of the moving object coordinate system from the detected distance data with reference to the position and orientation of the moving object 12, and converts the calculated position data into position data of the world coordinate system. It is a program. The unnecessary data erasing program 302d erases the distance data corresponding to the position data included in the cluster determined to be a dynamic object and the cluster where the dynamic object or the static object cannot be determined prior to the position estimation of the moving body 12. It is a program to do. The position estimation program 302e is a program for estimating the position (state) of the moving body 12 using the particle filter based on the distance data excluding the distance data deleted according to the unnecessary data erasing program 302d and the map data 304e. . The automatic navigation program 302f is a program for automatically moving the moving body 12 toward a destination (one or a plurality of passing points and end points) with reference to the map data 304e.

  Although illustration is omitted, other programs necessary for position estimation 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 position data 304c, cluster data 304d, map data 304e, and status data 304f. The position data 304c is position (point) data calculated from distance data stored in the distance data buffer 304a according to the position data calculation program 302c and coordinate-converted into the world coordinate system. Although detailed description is omitted, the distance data and the corresponding position data are associated with each other. The cluster data 304d is data for each cluster obtained by clustering the position data calculated from the distance data and subjected to coordinate conversion. The map data 304e is grid map data regarding the environment in which the moving body 12 moves. In the map data 304e, not only the grid map but also information indicating that the object is a static object corresponding to the grid corresponding to (the surface of) the static object is stored as described above. The state data 304f is data regarding the state (position and orientation) of the moving body 12 estimated according to the position estimation program 302e.

  Although illustration is omitted, the data storage area 304 stores other data necessary for position estimation, and is provided with a flag, a timer (counter), and the like necessary for position estimation.

  13 and 14 are flowcharts showing the overall processing of the CPU 30a shown in FIG. When given a start position, one or a plurality of passing points, and an end point from the outside, the CPU 30a starts the entire process as shown in FIG. 13, and drives the distance sensors 22a and 22b in step S1. At this time, the CPU 30a starts acquiring and storing distance data from the distance sensors 22a and 22b.

  In the next step S3, the first passing point is set. Here, the coordinates of the first passing point among the specified passing points are set. And in step S5, the autonomous navigation of the mobile 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 refers to the map data 304e to control the motor drivers 34a and 34b so as to go to the instructed passing point or end point, and the motor 36a. , 36b are driven.

  In subsequent step S7, 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 S9, position data is calculated from the distance data according to the position and orientation of the moving body 12. Here, as described above, the position data of the moving object coordinate system is calculated based on the position and orientation of the moving object 12 from the distance data detected by the distance sensors 22a and 22b. At this time, as described above, the attachment positions of the two distance sensors 22a and 22b are considered. In step S11, the position data calculated in step S9 is converted into position data in the world coordinate system.

  As shown in FIG. 14, in the following step S13, an unnecessary data erasing process (see FIGS. 15 and 16) described later is executed, and in step S15, a predetermined time has elapsed since the previous position estimation (S17) was performed. Determine whether or not. At this time, the CPU 30a counts time using a timer (not shown). In this embodiment, it is determined whether or not a predetermined time has elapsed. However, it may be determined whether or not a predetermined distance has been moved. However, the process of step S15 may be omitted.

  When the entire process is started and the determination process of step S15 is first executed, “YES” is determined because position estimation has never been performed.

  If “NO” in the step S15, that is, if a predetermined time has not elapsed since the previous position estimation is performed, the process returns to the step S7 shown in FIG. On the other hand, if “YES” in the step S15, that is, if a predetermined time elapses after the previous position estimation, the position and the angle are estimated with reference to the map in a step S17. Here, as described above, the CPU 30a estimates (corrects) the position and orientation of the moving body 12 by the particle filter using the distance data after erasing unnecessary data. Therefore, the state data 304f is updated by the process of step S17.

  In a succeeding step S19, it is determined whether or not the passing point has been reached. Here, the CPU 30a determines whether or not the position estimated in step S17 matches the set passing point. If “YES” in the step S19, that is, if the passing point is reached, the next passing point or end point is set in a step S21, and the process returns to the step S7. Therefore, movement control is performed so that the moving body 12 moves toward the next passing point or end point. On the other hand, if “NO” in the step S19, that is, if the passing point has not been reached, it is determined whether or not the end point has been reached in a step S23. Here, the CPU 30a determines whether or not the position estimated in step S17 matches the end point.

  If “NO” in the step S23, that is, if the end point has not been reached, the process returns to the step S7. On the other hand, if “YES” in the step S23, that is, if the end point is reached, the entire processing is ended.

  Although detailed description is omitted, the distance data and the rotation speed data used for estimating the position (execution of the process of step S17) are deleted from the buffers 304a and 304b.

  FIG. 15 and FIG. 16 are flowcharts showing the unnecessary data erasing process in step S13 shown in FIG. As shown in FIG. 15, when starting the unnecessary data erasing process, the CPU 30a generates a new cluster for the point of interest (position corresponding to the position data) in step S33. 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 distance data corresponding to the position data included in the cluster is deleted from the distance data buffer 304a, and the process proceeds to step S55. That is, distance data unnecessary for position estimation is deleted by executing the process 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, the static object and the dynamic object are discriminated from the measurement result of the distance sensor, the measurement result for the object other than the static object is deleted, and the movement is performed using only the measurement result for the static object. Since the position of the body is estimated, the influence of the measurement result of the dynamic object can be reduced as much as possible. That is, the position can be estimated as accurately as possible even in an environment where a dynamic object such as a human is present.

  Further, in this embodiment, unnecessary data can be erased only by converting the distance data detected by the distance sensor into position data, so there is no need 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, 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 this embodiment, the distance data is converted into the position data of the world coordinate system and clustered. However, the distance data is converted into the position data of the coordinate system (moving object coordinate system) based on the moving object. Clustering.

  Furthermore, 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 ... Position estimation 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 (5)

A position estimation device that estimates the position of a moving body based on map data corresponding to a map of a certain environment and a detection result of a distance sensor installed in the moving body moving in the environment,
Position data calculating means for calculating position data of a point at which a distance is measured from distance data detected by the distance sensor;
Clustering means for clustering the position data calculated by the position data calculating means into a plurality of clusters according to a predetermined rule;
Extraction means for extracting feature quantities for the dispersion of a plurality of points indicated by position data included in each of a plurality of clusters clustered by the clustering means;
A condition determining unit that determines whether or not the feature amount extracted by the extracting unit satisfies a predetermined condition; and corresponding to position data included in a cluster that is determined not to satisfy the predetermined condition by the condition determining unit A position estimation device comprising erasing means for erasing distance data.   The position estimation 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 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 position estimation apparatus according to claim 1 or 2 . A computer position estimation program for estimating the position of a moving object based on map data corresponding to a map of a certain environment and a detection result of a distance sensor installed in the moving object moving in the environment,
In the computer,
A position data calculating step for calculating position data of a point at which the distance is measured from the distance data detected by the distance sensor;
A clustering step of clustering the position data calculated in the position data calculation step into a plurality of clusters according to a predetermined rule;
An extraction step of extracting feature amounts for the dispersion of a plurality of points indicated by position data included in each of a plurality of clusters clustered in the clustering step;
A condition determination step for determining whether or not the feature amount extracted in the extraction step satisfies a predetermined condition; and a distance corresponding to the position data included in the cluster determined not to satisfy the predetermined condition in the condition determination step A position estimation program for executing an erasing step for erasing data. A computer position estimation method for estimating the position of a moving object based on map data corresponding to a map of a certain environment and a detection result of a distance sensor installed in the moving object moving in the environment,
The computer
(A) calculating position data of a point at which a distance is measured from distance data detected by the distance sensor;
(B) clustering the position data calculated in step (a) into a plurality of clusters according to a predetermined rule;
(C) a step of extracting feature quantities for the dispersion of a plurality of points indicated by the position data included in each of the plurality of clusters clustered in step (b);
(D) determining whether or not the feature quantity extracted in step (c) satisfies a predetermined condition; and (e) included in the cluster determined not to satisfy the predetermined condition in step (d). A position estimation method comprising: deleting distance data corresponding to the position data to be recorded.

Images