JP5909486B2 - Self-position / posture estimation system - Google Patents

Description

The present invention relates to a self-position / posture estimation system mounted on a moving body and having a function of estimating a self-position / posture.

There is an autonomous mobile robot system that incorporates a work system that performs operations such as conveying an article with an autonomous mobile robot within a predetermined area.
In such a system, it is very important for an autonomous mobile robot to realize a function for determining whether or not a position and orientation indispensable for autonomous movement is normally estimated. If the position / orientation estimation is not operating normally, this can be detected immediately, and the operation mode can be switched from normal running to a mode such as speed limit or emergency stop to deal with troubles.
Further, Patent Document 1 (Japanese Patent Laid-Open No. 2005-242409) discloses an autonomous mobile robot that moves to a destination while preventing a danger that causes an unsafe situation and avoiding a dangerous place.

  Patent Document 2 (Japanese Patent Application Laid-Open No. 2011-43405) includes a distance sensor that detects distance information with respect to an object in a detection region in order to perform self-position estimation with high accuracy, and is detected by the distance sensor. It is disclosed that the self-position is estimated based on the distance information with respect to the object. Japanese Patent Application Laid-Open No. 2011-65308 discloses a device that autonomously moves while estimating its own position based on measurement data of the measurement device.

JP 2005-242409 A JP 2011-43405 A JP 2011-65308 A

  In Patent Document 1, a sign provided in a moving environment, for example, a mark or pattern, an ultrasonic generator, a laser light emitting device, or the like is used, and an image sensor such as a CCD or CMOS sensor corresponding to each sign is used as a detection means. , There are ultrasonic receivers and laser light receiving elements, the position direction is indicated by the detection results, an error in self-position estimation can be detected, and danger avoidance is described. However, this method has problems such as increased work costs such as installation and registration of signs.

  Further, Patent Document 2 proposes a method that uses the degree of coincidence as a position / orientation estimation evaluation value, but it cannot be correctly evaluated when the same scenery such as a long corridor continues. Furthermore, Patent Document 3 proposes a method of operating while evaluating the reliability of position and orientation estimation using an error variance map, but assumes that the characteristics of the sensor with respect to the landmark are known, It is difficult to build a safe system in an unknown environment.

In order to solve the above problems, the present invention performs position and orientation estimation using a particle filter in a mobile robot, obtains a position and orientation estimation threshold based on particle dispersion as an amount for determining the position and orientation estimation state, and moves The purpose is to decelerate and stop the robot.
In order to solve the above problems, the present invention performs position and orientation estimation using a particle filter in a self-position and orientation estimation system mounted on a moving body such as a mobile robot, and determines the position and orientation estimation state. The position and orientation estimation threshold value is obtained as a quantity based on particle dispersion, and when the position and orientation estimation is not operating normally, information and signals indicating that the position and orientation estimation is not operating normally can be output. Objective.

In order to solve the above-described problems, the present invention is a self-position / posture estimation system that is mounted on a mobile body and estimates the position / posture of the mobile body, and compares measurement data from a distance sensor provided on the mobile body with map data. A position and orientation estimation unit that estimates the position and orientation of the moving body, and a position and orientation estimation evaluation value calculation unit that calculates an evaluation value of the reliability of the position and orientation estimation result of the position and orientation estimation unit,
The position and orientation estimation evaluation value calculation unit calculates the evaluation value of the reliability of the position and orientation estimation result of the position and orientation estimation unit by calculating the degree of coincidence between the measurement data from the distance sensor and the map data and the variance of the position and orientation estimation result I tried to do it.

According to the present invention, it is determined whether or not the position / orientation estimation of the mobile robot is performed normally. If the position / orientation estimation is not normal, the robot can be decelerated or stopped urgently. it can.
Further, according to the present invention, it is determined whether or not the position / orientation estimation of the moving body is performed normally. If not, information or a signal indicating that the position / orientation estimation is not performed normally is output to the moving body. It is possible to do.

The system block diagram of the autonomous mobile robot of this invention is shown. The map with obstacles and the route and measurement status of the mobile robot are shown. The measurement state when the mobile robot is traveling is shown. The measurement state when the mobile robot is traveling is shown. The figure explaining a particle filter is shown. The figure explaining the content of the operation mode determination part of a mobile robot is shown. The figure explaining the content of the operation mode determination part of a mobile robot is shown. The figure explaining the content of the operation mode determination part of a mobile robot when an obstruction moves is shown. The processing flowchart of the deceleration or stop based on a position and orientation estimation evaluation value is shown. The flowchart which performs map change when there exists environmental change is shown. It is a figure which shows the driving | running route of a mobile robot. 7 is a flowchart for determining a threshold value of a position / orientation estimation evaluation value when a mobile robot is tried. The system block diagram in the case of no odometry in the autonomous mobile robot of this invention is shown.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
In the following specification, a mobile robot and a self-position / posture estimation system mounted on a mobile body are described. Here, the name indicating the “self-position / posture estimation” will be described as follows.
“Self-position / posture estimation” means to estimate the position (x, y) and posture θ of a mobile robot or a mobile object equipped with a self-position / posture estimation system. Including the case where no.
In academic societies, in many cases, “posture” is omitted, and simply “self-position estimation” is written, and this name is also synonymous with “self-position / posture estimation” described in the following specification. is there.
The name in English will also be described below.
The English name is translated as “position and orientation estimation”.
“Position” corresponds to the position (x, y), and “orientation” corresponds to the posture θ.
In academic societies and the like, “orientation” may be omitted and simply referred to as “position estimation”, which is synonymous with “position and orientation estimation” and is described in the following specification.
Similarly, the names “pose estimation”, “position and posture estimation”, and “configuration estimation” are synonymous and are described in the following specification.
In the following description of the embodiments, a mobile robot will be described as an example of the mobile body.
The self-position / posture estimation system mounted on the mobile body will be described as a system including a position / posture estimation unit mounted on the mobile robot.
(Example 1)

FIG. 1 is a system block diagram of an autonomous mobile robot according to an embodiment of the present invention.
In FIG. 1, 1 is an autonomous mobile robot, 9 is a position and orientation estimation unit, 2 is a position and orientation estimation evaluation value calculation unit for evaluating the result of the position and orientation estimation unit, 3 is an operation mode determination unit that determines an operation mode, 5 is a laser distance sensor that measures distance, 6 is a map storage unit that stores a map that describes the characteristics of an obstacle, 7 is a position and orientation control unit that controls the position and orientation of the mobile robot, and 8a and 8b are wheel positions. It is an encoder that measures the number of rotations. Reference numeral 50 denotes an image display for displaying a map, the position and orientation of the robot on the map, the measurement data of the laser distance sensor, and the like.

The autonomous mobile robot 1 includes a laser distance sensor 5 for measuring the distance d to the obstacle, a map describing the characteristics of the obstacle stored in the map storage unit 6, an encoder 8a for calculating the number of rotations of the wheels, 8b, a position / orientation estimation unit 9 for estimating the position / orientation of the robot 1 based on the distance d22, the map 6, and the number of rotations; an evaluation value calculation unit 2 for evaluating the position / orientation estimation result; The operation mode determination unit 3 determines the operation mode based on the value, and the position / orientation control unit 7 controls the position / orientation of the mobile robot 1 based on the operation mode and the position / orientation estimation result.
Here, the position of the mobile robot 1 with respect to the reference coordinate system (world coordinate system) is (x, y), the posture of the mobile robot 1 with respect to the x-axis is θ, and collectively (x, y, θ). It will be expressed as

Next, the laser distance sensor 5 used in this embodiment will be described with reference to FIG.
In FIG. 3, 1 is a mobile robot, 61 is a wall, 14 is a work table which is an obstacle, 22 is distance data from the laser distance sensor 5 of the mobile robot to the obstacle, 23 is a robot after movement, 24 is its This represents the movement amount u.
The laser distance sensor 5 mounted on the mobile robot 1 measures a distance from an object around the mobile robot 1 with a resolution Δθ in a range of ± W (deg) (for example, W = 90, θ = 0.5). . This measurement data is called distance data d22. This data measures the shape of the measurement object around the mobile robot 1. The position and orientation of the mobile robot 1 can be estimated by collating the distance data d22 with the map information.

Next, the particle filter mounted on the position / orientation estimation unit 9 will be described with reference to FIGS.
In FIG. 2, 10 is a map representing an area where the mobile robot travels, and 11, 12, 13, 14 and 16 represent obstacles. Reference numerals 17 and 18 denote loading and unloading places, and reference numerals 32 and 33 denote workers.
Further, in the map shown in FIG. 2, the path between the unloading places 17 and 18 includes a path 19 on the lower path and a path on the right side, a path 20 on the path between the obstacles 11 and 13 and the obstacles 12 and 14, In addition, a path 21 from the left passage to the upper passage is shown.

In FIG. 4, 25 indicates the position of the mobile robot before movement. Reference numeral 27 denotes a state in which the movement amount ue26 has been moved.
Further, in FIG. 5, 28 is a particle before the mobile robot moves, 30 is a particle moved in this area by a movement amount ue29, and 31 is a particle with a large weight of dispersed particles. .
The particle filter can probabilistically merge the position and orientation estimation results from a plurality of sensors and reduce the estimation error. The position and orientation candidates of the mobile robot 1 are expressed by a plurality of candidates (xi, yi, θi); i = 1,. This is an effective means even when the position / orientation estimation result by the laser distance sensor has a plurality of solutions.

  The particle filter is executed in two steps: a prediction step and an update step. In the prediction step, the post-movement position of the mobile robot 1 is estimated using odometry calculated from the encoders 8a and 8b. In the update step, the position and orientation of the robot predicted based on the position estimation result using the laser distance sensor and the map are updated.

Hereinafter, the prediction step and the update step will be described in detail.
1) Prediction Step The prediction step will be described with reference to FIGS. Here, for simplicity of explanation, M = 1.
In FIG. 3, the mobile robot 1 moves from the initial position and orientation (x0, y0, θ0) by a movement amount u, and the distance from the measurement objects 61 and 62 by the laser distance sensor 5 at the position and orientation (x, y, θ) of 23. Represents the measured state. Here, it is assumed that the initial position and orientation (x0, y0, θ0) of the mobile robot 1 is known. In FIG. 4, the movement amount ue = (ue_x, ue_y, ue_θ) 26 is calculated from the initial position / posture (x0, y0, θ0) 25 using the values of the encoders 8a, 8b, and the post-movement position / posture is predicted from this value. Represents the state. At this time, the predicted value 27 is (x0 + ue_x, y0 + ue_y, θ0 + ue_θ) with respect to the position and orientation (x, y, θ) after movement. Since the value of this movement amount (ue_x, ue_y, ue_θ) includes errors such as wheel slip, a plurality of predicted values are generated in the prediction step using position and orientation candidates according to the error distribution of the encoders 8a and 8b. . The plurality of position / orientation candidates are represented by predicted particles (xp, yp, θp); p = 1,..., N, and are created using pseudorandom numbers or the like.
FIG. 5 shows predicted particles 30 (xp, yp, θp); p = 1,..., MN generated from the initial particles 28 (M = 5) based on the movement amount ue. It can be seen that the predicted particles 30 are distributed over a larger area than the initial particles 28 by the error of the encoders 8a and 8b.

2) Update Step Next, the update step will be described with reference to FIGS.
In the updating step, first, the degree of coincidence between the distance data d22 and the maps 10 and 14 is calculated for the predicted particles (xp, yp, θp); Specifically, assuming that the distance data d22 is measured by the position and orientation (xp, yp, θp), the number of points where the maps 10, 14 and the distance data d22 overlap is calculated. This value is called the matching degree R. The coincidence degree R is a high value if the true position / posture (x, y, θ) and the predicted particle (xp, yp, θp) are close to each other, and a low value if the coincidence is greatly deviated. FIG. 4 shows an example in which the degree of coincidence is calculated by fitting the distance data d22 from the laser distance sensor 5 of FIG. Since the amount of movement ue26 includes an error, it can be seen that the maps 10 and 14 are deviated if applied as they are.

  Next, by calculating the degree of coincidence R for all predicted particles (xp, yp, θp); p = 1,..., N, the position and orientation (xe, ye, θe) with the highest coincidence is calculated. Find out. Since this position / orientation (xe, ye, θe) will be close to the position / orientation (x, y, θ) observed by the laser distance sensor 5, this point is taken as the position / orientation estimation result of the robot, and (x0) in the next step , Y0, θ0).

  As shown in FIG. 5, when the initial position is not accurately known, the position and orientation of the robot are expressed by a plurality of particles. At this time, a prediction step is applied to each of the M initial particles (x0i, y0i, θ0i), and the generated predicted particles (xp, yp, θp); p = 1,. The degree of coincidence is calculated for it. Among them, the one with the highest degree of coincidence is adopted as the estimated position and orientation of the robot. Thereafter, the initial particle (x0i, y0i, θ0i) is updated by selecting the M point from the MN points according to the degree of coincidence.

  The above is the particle filter algorithm. In the update step, the one with the highest degree of coincidence calculated for (xp, yp, θp) was adopted as the estimation result. However, it should be noted here that there are cases where there are two particles having a matching degree that is the maximum value. In this situation, if the distance between the two points having the highest degree of coincidence is close, it does not matter whether either one is selected, but the problem is when the distance between the two points is far. In this case, which particle should be selected cannot be distinguished by the degree of coincidence. In some cases, the degree of coincidence is the same value at a plurality of points instead of two points, and becomes the maximum value. In such a situation, since the dispersion of estimation results becomes large, it is not preferable in terms of performance and safety that the mobile robot uses this value as it is for control. Therefore, in order to evaluate the reliability of the estimation result, it is necessary to define the position / orientation estimation evaluation value E. If this value E is used, the mobile robot 1 can select or estimate the position / orientation estimated value or decelerate / stop, and the mobile robot 1 can perform safer autonomous movement.

Here, since the degree of coincidence of a plurality of particles is maximized, and the distance between particles having the maximum degree of coincidence increases, the state is considered to be a state in which the dispersion value of the particles 31 is large. The variance σ around the average value of the particles is applied as the position / orientation estimation evaluation value E.
As an example in which the dispersion becomes large, the distance data d22 and the maps 10 and 14 do not coincide with each other when there is a change in the environment around the mobile robot 1 (the movement of the measurement object described on the map, a worker, etc.). May be widely distributed. Further, even when the distance data d22 and the maps 10 and 14 are in good agreement, particles are distributed over a wide range in the longitudinal direction of the corridor in a long corridor. In such a situation, it is possible to evaluate the reliability of the position / orientation estimation result by using particle dispersion.

  The position / orientation estimation evaluation value calculation unit 2 calculates the dispersion of particles. In this embodiment, the following evaluation formula is used.

E × E = σx × σx + σy × σy + α × σθ × σθ (Equation 1)
These are the sums of the standard deviations σx, σy, σθ of x, y, θ. α is a normalization constant, and is set to a value that equals the average value of the position variance and the attitude variance.
In this way, if the sum of variances is taken as the value of E, it can be made one-dimensional, and the evaluation is simplified. In addition, there is a method for individually evaluating eigenvalues of the covariance matrix and variances of x, y, and θ. Although the evaluation method is complicated, more detailed evaluation is possible.

Next, the operation mode determination unit 3 will be described in detail with reference to FIGS. 6, 7, and 8.
FIG. 6 shows the particles 31 when the workers 32 and 33 are standing in front of the mobile robot. FIG. 7 shows the particles 31 when the workers 32 and 33 are standing in front of the mobile robot.
FIG. 8 shows a case where the handicapped person moved by an earthquake or a person hit. FIG. 8A shows a state in which the distance of the mobile robot is being measured, and FIG. 8B shows the particle 31 at that time.

  FIG. 8 shows a state where a plurality of maximum matching degrees appear. FIG. 8A shows a situation in which the mobile robot 1 performs measurement by the laser distance sensor in a situation where the work table 14 has moved unintentionally due to contact of the workers 32 and 33 after the map is created. Yes. Since the map describes the arrangement of the work table 62 before movement, there is no position and orientation (x, y, θ) where the distance data d22 and the maps 10 and 14 completely coincide. FIG. 8B shows the result of position and orientation estimation performed by the particle filter in this state. A place with a high degree of coincidence is formed on the left side with respect to the moving direction of the mobile robot separately from the original position and this position. This is because the work table 14 has moved, the passage width becomes narrower, and the distance data d22 that matches the walls on both sides of the passage appears. In this situation, roughly divided, two patterns of 40 and 41 match and 40 and 42 match remain after the update step, and the particles are widely distributed. It is possible to evaluate that the value increases and the position / orientation estimation state is becoming unstable.

  6 and 7 show a case where workers 32 and 33 at the site are present in front of the moving mobile robot 1. In FIG. 6, about half of the distance data in front of the mobile robot 1 is hidden by the workers 32 and 33. At this time, the number of measurement points in the front-rear direction is reduced compared to the state of FIG. Therefore, since the particles selected in the update step are distributed more widely than usual in the front-rear direction of the mobile robot 1, the position / orientation estimation evaluation value E increases, so that the mobile robot 1 performs deceleration control, thereby enabling safer movement. It becomes possible.

  In FIG. 7, the workers 32 and 33 completely hide the front of the mobile robot 1, and it is almost impossible to estimate the front-rear direction coordinates of the mobile robot 1. At this time, the value of the position / orientation estimation evaluation value E is further deteriorated from the state of FIG. 6, so that the operation mode determination unit stops the mobile robot 1 safely based on this value.

  As a method for stopping the mobile robot 1 safely, an object detection area having a size proportional to the moving speed of the robot is provided in the robot travel method within the measurement range of the laser distance sensor, and the measurement object is detected in that area. Some of them are considered dangerous and stop. However, when this method is used, there is a problem that a wall is unnecessarily detected when the robot turns and noise is detected, which causes a reduction in work efficiency. The present invention is essentially a necessary stop process based on the reliability of the position estimation result, and it is a feature of the present invention that no noise is detected to cause an unnecessary stop process.

  In addition, since the mobile robot 1 has once exceeded the stop threshold, it cannot be restored until it is identified whether the cause is an operator blockage or an environmental change. At this time, it is also a feature that it is automatically switched to the manual mode on the assumption that the worker confirms and operates the state of the mobile robot 1.

FIG. 9 is a flowchart of processing for decelerating and stopping the mobile robot 1 based on the position / orientation estimation evaluation value E.
First, the mobile robot 1 loads the luggage loading / unloading location 17 or 18 and autonomously moves toward the destination (step 201). While moving, the mobile robot 1 performs sensing and estimates position and orientation (step 202). Next, the position / orientation estimation evaluation value E is calculated for the position / orientation estimation result (step 203).
The mobile robot 1 determines whether the destination has been reached from the position / orientation estimation result (step 204). If it has reached, the operation is terminated, and if not, the operation mode is changed based on the position / orientation estimation evaluation value E, and the position / orientation estimation evaluation value E is compared with the threshold E1 (step 205). Here, if the position / orientation estimation evaluation value E is larger than the threshold value E1, the mobile robot 1 shifts from the normal travel mode to the deceleration mode (step 206).
If the evaluation value E is smaller than the threshold value E1, the process returns to step 201 for autonomous movement.
In the deceleration mode (step 206), the position / orientation estimation evaluation value E and the threshold value E2 are compared, and if the evaluation value E is large, that is, if the stop threshold value is exceeded, autonomous movement is virtually impossible. Therefore, the process shifts to the manual mode (step 208).

If the evaluation value E is smaller than the threshold value E2, the operation mode is the deceleration mode and the autonomous movement is continued (return to step 201).
When the manual mode is switched (step 208), the operator is notified of the switching (step 209), and the mobile robot 1 stops.
In this embodiment, the deceleration threshold value is provided in only one stage, but it is also possible to provide deceleration in multiple stages for deceleration.

FIG. 10 is a flowchart showing a return procedure of the mobile robot 1 when the stop threshold is exceeded.
First, the mobile robot 1 performs position and orientation estimation (step 301). Next, an evaluation value for the position / orientation estimation result in step 301 is calculated (step 302). Next, the mobile robot 1 notifies the worker / manager that it is currently stopped (step 303). The worker who has received the notification confirms the image display 50 attached to the mobile robot 1 and confirms that the position / orientation estimation evaluation value E is smaller than the stop threshold E2 and that the distance data d and the maps 10 and 14 are the same. It confirms that it has done (step 304). At this time, the position of the map 6 and the mobile robot 1, the distance data d, the evaluation value E, the matching degree R, and the like are displayed on the image display. The distance data is color-coded according to the points that overlap the map and the points that do not overlap, and is characterized by the fact that the point where the environment changes is clear.

  After the worker confirms the current state in step 304, if the cause of exceeding the threshold is the worker, the manual mode is canceled and the mobile robot 1 is returned to work (step 308). When the distance data d and the maps 10 and 14 do not match and the cause is recognized as the movement of the measurement object such as a work table, the worker / administrator views the map around the mobile robot 1 stopped. Update (step 306). For updating the map, the mobile robot 1 switched to the manual mode may be operated to collect the distance data d, or an instrument dedicated to map creation may be used. At this time, the update of the map may be performed on the robot while confirming the display of the mobile robot 1, or only the distance data d can be taken home and the map can be created on another PC. Thereafter, the mobile robot 1 reads the updated map and returns to work.

FIG. 11 shows the work site environment of the robot 1. There are loading and unloading places 17, 18 at the work site, and movement paths 19, 20, 21 connecting them are set. In order for the robot 1 to move autonomously in this environment, it is first necessary to create a map of the site. Next, the robot 1 performs a trial run on the site to confirm whether the mapping is correctly performed.
A threshold E is set based on the measurement data in this trial run. Although the threshold value should be determined from the sensor characteristics, it is difficult to grasp all combinations because the sensor characteristics depend on the material and color of the measurement object. Therefore, by actually measuring, a threshold suitable for the site can be set, so that safer autonomous movement is possible.

FIG. 12 shows a flowchart of the threshold determination process.
First, the mobile robot 1 creates an environmental map (step 101). Next, the mobile robot 1 performs autonomous movement along the route on which the robot is actually operated, using the map created in step 101 (step 102). At this time, the mobile robot 1 actually estimates the position and orientation, and moves while saving the result in the map storage unit 6. At this time, when the mobile robot 1 runs through the movement route without any problem, data processing for determining a threshold value is performed using the position / orientation estimation evaluation value stored in the storage unit 6 (step 103). When the threshold is determined, the vehicle is operated in actual driving (step 104).

  Specifically, when the mobile robot 1 completes the trial run without any problem, the threshold value is set by using an E statistic such as the maximum value Emax of the position / orientation estimation evaluation value E calculated during the travel. A value of 2Emax> E1> Emax is used as a deceleration threshold, and E2 = 2Emax is used as a stop threshold. The reason why such a value is used as the threshold value is that the value up to Emax is a value generated in the trial run immediately after the map creation, and it is guaranteed that the vehicle can normally run within this range. This is the preparation stage in the actual operation of the mobile robot 1.

In this embodiment, the particle filter estimation result is the particle having the highest degree of coincidence, but the average value of the particles may be used as the position and orientation estimation result.
In this embodiment, the dispersion of particles is used as the position / orientation estimation evaluation value. However, a statistic indicating the spread of particles, for example, the rectangle including all particles, the area of an ellipse, and the like can be directly applied to the present invention. is there.

Also, the degree of coincidence R can be used as the evaluation value. The degree of coincidence represents the degree of environmental change as it is, and is an important value that gives information on the surrounding environment of the mobile robot 1.
However, as described in the present embodiment, since the reliability of the position estimation result cannot be evaluated only by the degree of coincidence, the evaluation is performed including the variance. As an evaluation method, the results of individually evaluating the degree of coincidence R and the variance may be integrated, or when multidimensional evaluation is troublesome, an evaluation function that can be evaluated one-dimensionally may be created. As a specific example of the evaluation function, the following equation is used.

E = R / (1 + σ / 2σmax) (Equation 2)
In this equation, σmax represents the maximum value of the standard deviation determined by the trial run in FIG. This formula is such that when the variance value is a good value, the evaluation is performed with the degree of coincidence, and when the degree of coincidence is good, the evaluation is performed with the dispersion value. The standard deviation σ is best when it is 0, but at this time, E = R. At this time, E = 0.5 represents that the measured object is 50%. Further, R is best, and at this time, the evaluation value is E = 1 / (1 + σ / 2σmax), and when σ = 2σmax, E = 0.5. Therefore, if the stop threshold is set to 0.5, a better evaluation including the degree of coincidence can be performed.

  In this embodiment, the position / orientation estimation evaluation value has been described by applying a particle filter as the position / orientation estimation method. However, the Kalman filter is used as the estimation method, and the estimation / error estimation covariance matrix is used as the position / orientation estimation evaluation value. May be used. Further, even in a state where there is no odometry, that is, a state where prediction is not possible at all, the moving amount u can be regarded as an error, and a particle filter and a Kalman filter can be applied.

  Furthermore, a search region (x0 ± Lx, y ± Ly, θ0 ± Lθ) centered on the initial position and orientation (x0, y0, θ0) is provided, and the inside of the search region is (x0 ± k1Δx, y0 ± k2Δy, θ0). (Where k1, k2, and k3 are integers) and the degree of coincidence is calculated on the lattice points expressed in a discrete manner, the position / orientation estimation evaluation value similar to that of the particle filter can be defined. . Specifically, the principal component analysis in the image processing is performed in the search region using the degree of coincidence as a weight, and the principal component value may be used. This will be described in detail below.

FIG. 13 is a configuration diagram of a position and orientation estimation system in a state where there is no odometry. Unlike FIG. 1, the input of odometry is not input to the position and orientation processing unit. At this time, since it is impossible to predict the position and orientation of the robot after movement, the calculation cost increases, and it is necessary to use a high-performance CPU.
However, it is possible to use wheels without an encoder, and there is an advantage that the position and orientation estimation system can be easily used.
As an example of the position / orientation estimation method in this case, a method for calculating a position / orientation estimation evaluation value will be described using matching processing as an example.
In the matching process, the degree of coincidence is not calculated for a randomly selected position and orientation as in a particle filter, but a limited area centered on the initial position and orientation (x0, y0, θ0) is used as a lattice point. This is a method of detecting the point with the highest degree of coincidence by calculating the degree of coincidence with respect to the position and orientation selected in a shape and setting it as the estimated position and orientation. As the initial position and orientation (x0, y0, θ0), the previous position and orientation estimation result is used.

A specific processing method is as follows.
First, a search region (x0 ± Lx, y ± Ly, θ0 ± Lθ) centered on the initial position and orientation (x0, y0, θ0) is provided, and a jump point (X, Y, Θ) = (X The degree of coincidence is calculated on a lattice point expressed by x0 ± k1Δx, y0 ± k2Δy, θ0 ± k3Δθ).
Here, Lx, Ly, and Lθ are positive real numbers, and are set to a size that considers the moving speed of the robot. As described above, k1, k2, and k3 are integers, and Δx, Δy, and Δθ represent grid point widths. At this time, the degree of coincidence at each point is represented by R (X, Y, Θ), and the highest value (X, Y, Θ) is taken as the position and orientation estimation result. The above is the outline of the matching process.

Next, a method for calculating the position / orientation evaluation value will be described. When matching processing is performed in the state of FIG. 7, the worker in front of the robot blocks the laser beam. For this reason, only the walls on both sides of the robot coincide with each other, and the degree of coincidence with respect to the position and orientation along the wall is relatively high in R (X, Y, Θ).
Conversely, the calculation result of R (X, Y, Θ) indicates that the estimation result is indefinite in the direction in which values having a high degree of coincidence are widely distributed. The position / orientation evaluation value similar to that of the particle filter can be defined by calculating the degree of dispersion of the degree of coincidence. In order to detect the direction in which the degree of coincidence is strongly dispersed, first, the R covariance matrix B (3 × 3 matrix) is calculated.

B = ΣR (X, Y, Θ) (X−μ _x , Y−μ _y , Θ−μ _θ ) ^T (X−μ _x , Y−μ _y , Θ−μ _θ ) (Equation 3)
(The superscript T represents the transpose of the vector. Μ _x , μ _y, μ _θ represent the average value weighted by R (X, Y, Θ) of X, Y, Θ. Σ represents the lattice in the search region. Represents the sum of points (X, Y, Θ).)
Next, if B is subjected to eigenvalue decomposition, it is expressed as follows.

B = λ ₁ v ₁ ^T v ₁ + λ ₂ v ₂ ^T v ₂ + λ ₃ v ₃ ^T v ₃ (Equation 4)
The v ₁ represents the direction in which the degree of coincidence is dispersed a 3-dimensional eigenvector (row vector). λ ₁ ≧ λ ₂ ≧ λ ₃ are eigenvalues, and each λ ₁ represents how much the degree of coincidence is dispersed in the direction of v ₁ . Such a method is also called principal component analysis, where λ ₁ is called a first principal component, λ ₂ is called a second principal component, and so on.
By using this principal component value λ, it is possible to detect a state in which position and orientation cannot be estimated as in the particle filter.
The threshold for evaluation is determined in the same manner as for the particle filter.

DESCRIPTION OF SYMBOLS 1 ... Autonomous mobile robot 2 ... Position / orientation estimation evaluation value calculation part 3 ... Operation mode determination part 5 ... Laser distance sensor 6 ... Map storage part 7 ... Position / orientation control part 8a, 8b ... Encoder 9 ... Position / orientation estimation part 10 ... Map 14. Work table 19, 20, 21 Movement path 22 Distance data d 24 Movement amount u
31 ... Particle 32, 33 ... Worker 50 ... Image display

Claims (4)

A self-position / posture estimation system that is mounted on a mobile body and estimates the position / posture of the mobile body,
A position and orientation estimation unit that estimates the position and orientation of the moving body by collating measurement data from a distance sensor provided on the moving body with map data;
A position and orientation estimation evaluation value calculation unit that calculates an evaluation value of reliability of the position and orientation estimation result of the position and orientation estimation unit, and
The position / orientation estimation evaluation value calculation unit evaluates the reliability of the position / orientation estimation result of the position / orientation estimation unit by calculating the degree of coincidence between the measurement data from the distance sensor and the map data and the variance of the position / orientation estimation result. A position and orientation estimation system characterized by calculating a value. The position / orientation estimation system according to claim 1, wherein measurement data from the distance sensor acquired in the moving body is used to create map data. The position and orientation estimation system according to claim 1, further comprising a movement amount estimation unit that estimates a movement amount based on measurement data from an encoder that measures the movement amount of the moving body. The position / orientation estimation system according to claim 1, wherein the moving body decelerates or stops according to the evaluation value.

Images