JP2020008461A - Autonomous moving body location estimation device - Google Patents

Abstract

To provide an autonomous moving body location estimation device that can upgrade estimation accuracy of a self-location of the autonomous moving body.SOLUTION: A location estimation device 1 comprises: a first self-location estimation meter 3 and second self-location estimation meter 4 that estimate a self-location of a folk lift 2, using a different self-location estimation technology; and a final self-location estimation unit 12 that finally estimates a self-location of the folk lift 2 on the basis of two location estimation values of the folk lift 2 each obtained by the first self-location estimation meter 3 and second self-location estimation meter 4. The final self-location estimation unit 12 is configured to: set an error variance (weight coefficient) of the first self-location estimation meter 3 and second self-location estimation meter 4 on the basis of an ambient environment of the folk lift 2; and finally estimate the self-location of the folk lift 2, using the error variance of the first self-location estimation meter 3 and second self-location estimation meter 4.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a position estimation device for an autonomous mobile object.

  As a position estimating apparatus for an autonomous mobile object, for example, as described in Patent Literature 1, a technique for estimating a self-position using a simultaneous position estimation map construction (SLAM) for constructing and using a map is known. Have been.

JP 2014-222550 A

  However, in the above-described related art, the self-position estimation result may be shifted depending on the surrounding environment of the autonomous mobile body, and thus it is difficult to estimate the self-position of the autonomous mobile body with high accuracy.

  An object of the present invention is to provide an autonomous moving object position estimating device that can improve the accuracy of estimating the position of the autonomous moving object.

  One aspect of the present invention is an autonomous mobile position estimating device that estimates the position of an autonomous mobile object, and includes a plurality of self-position estimators that estimate the self-position of the autonomous mobile object using different self-position estimation techniques. And a final self-position estimating unit for finally estimating the self-position of the autonomous mobile based on the position estimation values of the plurality of autonomous mobiles obtained by the plurality of self-position estimators, respectively. The unit sets the weighting factors of the plurality of self-position estimators based on the surrounding environment of the autonomous mobile body, and finally estimates the self-position of the autonomous mobile body using the weighting coefficients of the plurality of self-location estimators.

  In such a position estimating apparatus, the self-position of the autonomous mobile is estimated by a plurality of self-position estimators having different self-position estimation techniques, and the position estimation value of each autonomous mobile obtained by the plurality of self-position estimators , The self-position of the autonomous mobile object is finally estimated. At this time, the weighting factor of each self-position estimator is set based on the surrounding environment of the autonomous mobile, and the self-position of the autonomous mobile is finally estimated using the weighting factor of each self-position estimator. Thereby, the estimation accuracy of the self-position of the autonomous mobile body is improved.

  The final self-position estimating unit may set the weighting factor based on the environment of the self-position of the autonomous mobile that was last estimated last time. In such a configuration, the current surrounding environment of the autonomous mobile body can be obtained with high accuracy, so that the weight coefficient of each self-position estimator is appropriately set. Thereby, the estimation accuracy of the self-position of the autonomous mobile body is further improved.

  The self-position estimator estimates the two-dimensional position and orientation of the autonomous mobile as the self-position of the autonomous mobile, and the final self-position estimator calculates the two-dimensional position and orientation of the autonomous mobile as the weighting factor of the self-position estimator. May be set. In such a configuration, not only the two-dimensional position of the autonomous mobile body but also the orientation of the autonomous mobile body is taken into account, so that the estimation accuracy of the self-position of the autonomous mobile body is further improved.

  The number of self-position estimators is two, and one of the two self-position estimators determines the self-position of the autonomous mobile body using a self-position estimation technique in which the estimation accuracy in indoors is higher than the estimation accuracy in outdoors. The estimation may be performed, and the other of the two self-position estimators may estimate the self-position of the autonomous mobile body using a self-position estimation technique in which the estimation accuracy outdoors is higher than the estimation accuracy indoors. In such a configuration, when the autonomous mobile body moves outdoors and indoors, the appropriate weight coefficient of each self-position estimator corresponding to the outdoors and indoors is set. Is further improved.

  ADVANTAGE OF THE INVENTION According to this invention, the estimation accuracy of the self-position of an autonomous mobile body can be improved.

It is a schematic structure figure showing the position estimating device of the autonomous mobile concerning one embodiment of the present invention. 5 is a flowchart illustrating details of a self-position calculation process performed by a self-position calculation unit illustrated in FIG. 1. 3 is a flowchart illustrating details of an outlier removal processing procedure executed by the outlier removal unit illustrated in FIG. 1. 5 is a flowchart illustrating details of a self-position estimation process performed by a final self-position estimator illustrated in FIG. 1. 6 is a flowchart illustrating a modification of the outlier removal processing procedure illustrated in FIG. 3.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the drawings, the same or equivalent elements have the same reference characters allotted, and overlapping description will be omitted.

  FIG. 1 is a schematic configuration diagram illustrating an apparatus for estimating the position of an autonomous moving object according to an embodiment of the present invention. In FIG. 1, a position estimating device 1 of the present embodiment is a device for estimating the position of a forklift 2 when performing automatic operation of a forklift 2 which is an autonomous moving body. The position estimating device 1 is mounted on a forklift 2.

  The position estimation device 1 includes a first self-position estimator 3, a second self-position estimator 4, a wheel encoder 5, a gyro sensor 6, a controller 7, and a display 8. That is, the position estimating apparatus 1 includes a plurality of (here, two) self-position estimators.

  The first self-position estimator 3 estimates the self-position of the forklift 2 using Simultaneous Localization and Mapping (SLAM). SLAM is a self-position estimation technique for performing self-position estimation using sensor data and map data. The SLAM uses a laser range scanner or the like as a sensor to simultaneously perform self-position estimation and create an environmental map. In SLAM, the result of self-location estimation varies depending on the presence or absence of a building. Therefore, SLAM is a self-position estimation technique in which the estimation accuracy indoors is higher than the estimation accuracy outdoors. The first self-position estimator 3 estimates the self-position of the forklift 2 as an absolute position.

  The second self-position estimator 4 estimates the self-position of the forklift 2 using a GNSS (Global Navigation Satellite System). GNSS is a self-position estimation technology that performs self-position estimation using satellites. In the GNSS, the self-position estimation result varies depending on the state of the satellite. Therefore, GNSS is a self-position estimation technology in which the estimation accuracy outdoors is higher than the estimation accuracy indoors. The second self-position estimator 4 estimates the self-position of the forklift 2 as an absolute position.

  The first self-position estimator 3 and the second self-position estimator 4 estimate the two-dimensional position and orientation of the forklift 2 as the self-position of the forklift 2. Here, the self-position estimated by the first self-position estimator 3 and the second self-position estimator 4 is referred to as a position estimation value. The self-position estimation by the first self-position estimator 3 and the second self-position estimator 4 is performed every unit time (for example, several milliseconds).

  The wheel encoder 5 is a sensor attached to the wheel of the forklift 2 and measures the speed of the forklift 2. The gyro sensor 6 is a sensor that is attached to the body of the forklift 2 and measures the angular velocity of the forklift 2.

  The controller 7 includes a CPU, a RAM, a ROM, an input / output interface, and the like. The controller 7 inputs the self-position estimation results of the first self-position estimator 3 and the second self-position estimator 4 and the measurement values of the wheel encoder 5 and the gyro sensor 6, performs predetermined processing, and performs Estimate the position. The controller 7 has a self-position calculating unit 10, an outlier removing unit 11, and a final self-position estimating unit 12.

  The self-position calculation unit 10 calculates the self-position of the forklift 2 based on the measurement values of the wheel encoder 5 and the gyro sensor 6. The wheel encoder 5, the gyro sensor 6, and the self-position calculator 10 have a sensor provided in the forklift 2 and constitute a self-position detector 13 for detecting the self-position of the forklift 2. The self-position detecting unit 13 detects the two-dimensional position and direction of the forklift 2 as the self-position of the forklift 2. Here, the self-position detected by the self-position detection unit 13 is referred to as a position detection value.

  The outlier removing unit 11 compares the position estimation value of the forklift 2 obtained by the first self-position estimator 3 and the second self-position estimator 4 with the position detection value of the forklift 2 obtained by the self-position detection unit 13. In comparison, outliers of the position estimation value of the forklift 2 are extracted and removed.

  The final self-position estimating unit 12 finally estimates the self-position of the forklift 2 based on the position estimation values of the two forklifts 2 obtained by the first self-position estimator 3 and the second self-position estimator 4, respectively. I do. At this time, the final self-position estimating unit 12 finally estimates the self-position of the forklift 2 based on the position estimated value of the forklift 2 from which outliers have been removed by the outlier removing unit 11. Further, the final self-position estimating unit 12 sets the weighting factors of the first self-position estimator 3 and the second self-position estimator 4 based on the surrounding environment of the forklift 2, and uses the weighting coefficients to determine the self-position of the forklift 2. The position is finally estimated.

  The display 8 displays the self-position of the forklift 2 finally estimated by the final self-position estimating unit 12. The display 8 displays, for example, the two-dimensional position and orientation of the forklift 2 on a screen.

  FIG. 2 is a flowchart illustrating details of the self-position calculation processing procedure executed by the self-position calculation unit 10. In FIG. 2, the self-position calculating unit 10 first obtains the measured values of the wheel encoder 5 and the gyro sensor 6 (step S101).

  Next, the self-position calculator 10 calculates a position detection value of the forklift 2 based on the measurement values of the wheel encoder 5 and the gyro sensor 6 (step S102). Specifically, the self-position calculating unit 10 obtains the two-dimensional position (the position of the XY coordinates) of the forklift 2 by integrating the speed of the forklift 2 measured by the wheel encoder 5. Further, the self-position calculator 10 calculates the direction of the forklift 2 by integrating the angular velocity of the forklift 2 measured by the gyro sensor 6. Next, the self-position calculating unit 10 outputs the detected position value of the forklift 2 to the outlier removing unit 11 (step S103).

  FIG. 3 is a flowchart illustrating details of the outlier removal processing procedure executed by the outlier removal unit 11. In FIG. 3, the outlier removing unit 11 first obtains the position estimated value of the forklift 2 obtained by the first self-position estimator 3 and the second self-position estimator 4 (step S111).

  Subsequently, the outlier removing unit 11 calculates the position estimation value of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4 and the first self-position estimator 3 and the second self-position estimation. The relative position (x, y, θ) with the position estimation value of the forklift 2 obtained last time is calculated by the container 4 (step S112). The position estimation value of the forklift 2 obtained last time is the self-position of the forklift 2 estimated unit time ago and is stored in the memory.

  The relative position (x, y, θ) may be calculated as, for example, an average value of the position estimation value obtained by the first self-position estimator 3 and the position estimation value obtained by the second self-position estimator 4. Alternatively, one of the position estimation value obtained by the first self-position estimator 3 and the position estimation value obtained by the second self-position estimator 4 which is closer to the previously calculated value may be adopted.

Subsequently, the outlier removal unit 11 acquires the position detection value of the forklift 2 obtained by the self-position calculation unit 10 (Step S113). Subsequently, the outlier removing unit 11 sets the relative position (x _d , Y _d , θ _d ) are calculated (step S114). The position detection value of the forklift 2 obtained last time is the self-position of the forklift 2 detected unit time ago and is stored in the memory.

  Subsequently, the outlier removal unit 11 sets thresholds δx, δy, δθ based on the surrounding environment of the forklift 2 (step S115). The thresholds δx and δy are thresholds related to the two-dimensional position (x-axis position, y-axis position) of the forklift 2. The threshold value δθ is a threshold value related to the direction of the forklift 2. The surrounding environment of the forklift 2 is the environment of the self-position of the forklift 2 previously estimated by the final self-position estimating unit 12. The surrounding environment of the forklift 2 can be grasped by the first self-position estimator 3 using SLAM for creating an environment map.

  For example, when the forklift 2 travels outdoors, which is slippery due to rain or the like, the wheel encoder 5 has a large influence of the slip, so that the measurement value of the wheel encoder 5 tends to have an error, but the gyro sensor 6 has a small influence of the slip. In addition, an error is hardly generated in the measurement value of the gyro sensor 6. Therefore, the threshold values δx and δy are set larger (slower) outdoors than indoors. For example, the thresholds δx and δy are set to 1 m outdoors and the thresholds δx and δy are set to 0.5 m indoors. The threshold value δθ is set equal between outdoor and indoor. For example, the threshold value δθ is set to 0.3 rad outdoors and indoors. Thus, the threshold values δx, δy, δθ are changed according to the surrounding environment of the forklift 2.

  When the forklift 2 travels indoors and outdoors with a large temperature difference, such as inside and outside of a refrigerator, an error tends to occur in the measurement value of the gyro sensor 6. For this reason, the threshold value δθ is set to be larger (slower) indoors than outdoors.

Subsequently, the outlier removing unit 11 calculates the calculation formula f () regarding the difference between the relative position (x _d , y _d , θ _d ) calculated in step S114 and (x, y, θ) calculated in step S112. x), f (y) and f (θ) are calculated (step S116). The calculation formulas f (x), f (y), and f (θ) are represented by the following formulas.

  Subsequently, the outlier removing unit 11 determines whether the calculated values of the calculation formulas f (x), f (y), and f (θ) are larger than the thresholds δx, δy, and δθ, respectively, as shown in the above expression. It is determined whether or not it is (step S117).

  When the outlier removing unit 11 determines that the calculated values of the calculation expressions f (x), f (y), and f (θ) are larger than the threshold values δx, δy, δθ, the first self-position estimator 3 and The position estimation value of the forklift 2 obtained this time by the second self-position estimator 4 is removed as an outlier (step S118). At this time, the outlier removing unit 11 determines that when at least one of the calculated values of the calculation formulas f (x), f (y), and f (θ) is larger than the threshold values δx, δy, δθ, If at least one is satisfied, the position estimation value of the forklift 2 obtained this time is regarded as an outlier.

  The outlier removing unit 11 determines that all of the calculated values of the calculation formulas f (x), f (y), and f (θ) are not larger than the threshold values δx, δy, δθ. When it is determined, the position estimation value of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4 is output to the final self-position estimator 12 (step S119).

  FIG. 4 is a flowchart illustrating details of the self-position estimation processing procedure executed by the final self-position estimation unit 12. Here, the final self-position estimating unit 12 combines the position estimation values of the two forklifts 2 obtained by the first self-position estimator 3 and the second self-position estimator 4, respectively, and uses the linear minimum variance estimation method. Then, the self-position of the forklift 2 is finally estimated.

  In FIG. 4, the final self-position estimating unit 12 first obtains the latest position estimation value (xs, ys, θs) of the forklift 2 obtained by the first self-position estimator 3 and obtains the latest position estimation value by the second self-position estimator 4. The latest estimated position value (xg, yg, θg) of the forklift 2 obtained is obtained (step S121).

Subsequently, based on the surrounding environment of the forklift 2, the final self-position estimating unit 12 calculates the error variance (σsx ² , σsy ² , σsθ ² ) of the first self-position estimator 3 and the second self-position estimator 4 Error variance (σgx ² , σgy ² , σgθ ² ) is set (step S122). The surrounding environment of the forklift 2 is the environment of the self-position of the forklift 2 previously estimated by the final self-position estimating unit 12. The error variance is a weight coefficient. The error variances σsx ² and σsy ² and the error variances σgx ² and σgy ² are weight coefficients related to the two-dimensional position (x-axis position, y-axis position) of the forklift 2. Error variance Shigumaesushita ² and error variance Shigumajishita ² is a weighting factor according to the orientation of the forklift 2.

  At this time, since the SLAM estimation accuracy is higher than the GNSS estimation accuracy indoors, the error variance of the first self-position estimator 3 is set lower than the error variance of the second self-position estimator 4. For example, the error variance of the first self-position estimator 3 is set to 4, and the error variance of the second self-position estimator 4 is set to 36. Since the GNSS estimation accuracy is higher than the SLAM estimation accuracy outdoors, the error variance of the second self-position estimator 4 is set lower than the error variance of the first self-position estimator 3. For example, assume that the error variance of the first self-position estimator 3 is 36, and the error variance of the second self-position estimator 4 is 4. In the middle between indoors and indoors, the error variance of the first self-position estimator 3 and the error variance of the second self-position estimator 4 are set to be equal. For example, the error variances of the first self-position estimator 3 and the second self-position estimator 4 are both set to 25. In addition, the middle between indoors and indoors is, for example, a place where there is no roof but there is a pillar near a building.

Subsequently, the final self-position estimating unit 12 updates the latest position estimation value (xs, ys, θs) of the forklift 2 obtained by the first self-position estimator 3 and the latest position estimation value obtained by the second self-position estimator 4. Of the forklift 2 (xg, yg, θg), the error variance (σsx ² , σsy ² , σsθ ² ) of the first self-position estimator 3 and the error variance (σgx ² , Using σgy ² , σgθ ² ), the final self-position (x _est , y _est , θ _est ) of the forklift 2 is estimated by the following equation.

  As described above, in the present embodiment, the self-position of the forklift 2 is estimated by the first self-position estimator 3 and the second self-position estimator 4 having different self-position estimation techniques. The self-position of the forklift 2 is finally estimated based on the position estimation value of each forklift 2 obtained by the second self-position estimator 4. At this time, the error variance (weight coefficient) of the first self-position estimator 3 and the second self-position estimator 4 is set based on the surrounding environment of the forklift 2, and the first self-position estimator 3 and the second self-position estimation The self-position of the forklift 2 is finally estimated using the error variance of the device 4. As a result, the accuracy of estimating the position of the forklift 2 is improved. As a result, when the forklift 2 is automatically operated, the forklift 2 can run stably.

  Further, in the present embodiment, the error variance of the first self-position estimator 3 and the second self-position estimator 4 is based on the environment of the self-position of the forklift 2 that was last estimated by the last self-position estimator 12 last time. Is set. With such a configuration, the current surrounding environment of the forklift 2 can be obtained with high accuracy, so that the error variances of the first self-position estimator 3 and the second self-position estimator 4 are appropriately set. Thereby, the accuracy of estimating the self-position of the forklift 2 is further improved.

  In the present embodiment, the first self-position estimator 3 and the second self-position estimator 4 estimate the two-dimensional position and orientation of the forklift 2 as the self-position of the forklift 2, and the final self-position estimator 12 An error variance related to the two-dimensional position and orientation of the forklift 2 is set as an error variance of the first self-position estimator 3 and the second self-position estimator 4. As described above, not only the two-dimensional position of the forklift 2 but also the orientation of the forklift 2 is considered, so that the estimation accuracy of the self-position of the forklift 2 is further improved.

  Further, in the present embodiment, the first self-position estimator 3 estimates the self-position of the forklift 2 using a self-position estimation technique in which the indoor estimation accuracy is higher than the outdoor estimation accuracy, and the second self-position estimator 3. The estimator 4 estimates the self-position of the forklift 2 using a self-position estimation technique in which the estimation accuracy outdoors is higher than the estimation accuracy indoors. With such a configuration, when the forklift 2 moves outdoors and indoors, the error variances of the first self-position estimator 3 and the second self-position estimator 4 appropriate for the outdoors and indoors are set. Therefore, the estimation accuracy of the self-position of the forklift 2 is further improved.

  FIG. 5 is a flowchart showing a modification of the outlier removal processing procedure shown in FIG. In FIG. 5, the outlier removing unit 11 executes steps S111 to S114, as in the flowchart shown in FIG.

  After executing step S114, the outlier removing unit 11 sets a threshold δ and weighting factors α, β, γ based on the surrounding environment of the forklift 2 (step S115A). The weight coefficient α is a weight coefficient related to the x-axis position of the forklift 2. The weight coefficient β is a weight coefficient related to the y-axis position of the forklift 2. The weight coefficient γ is a weight coefficient related to the direction of the forklift 2. At this time, for example, the weighting factors α and β are set to be smaller in an outdoor area that is slippery due to rain or the like than in an indoor area.

Subsequently, outlier removal unit 11, the relative position calculated in step _{S114 (x d, y d,} θ d) and calculated in step S112 (x, y, θ) the equation f regarding the difference between The calculation is performed (step S116A). The calculation formula f is represented by the following formula. The calculation formula f is a formula for adding variables relating to the two-dimensional position and orientation of the forklift 2.

  Subsequently, the outlier removing unit 11 determines whether the calculated value of the calculation formula f is larger than the threshold value δ, as shown in the above formula (step S117A). When the outlier removing unit 11 determines that the calculated value of the calculation formula f is larger than the threshold δ, the first self-position estimator 3 and the second self-position estimator 4 estimate the position of the forklift 2 obtained this time. The value is removed as an outlier (step S118).

  When the outlier removing unit 11 determines that the calculated value of the calculation formula f is not larger than the threshold δ, the position of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4 is determined. The estimated value is output to the final self-position estimating unit 12 (step S119).

  Note that the present invention is not limited to the above embodiment. For example, in the above embodiment, the final self-position estimating unit 12 fuses the position estimation values of the two forklifts 2 obtained by the first self-position estimator 3 and the second self-position estimator 4, respectively, and performs linear minimum variance estimation. Although the self-position of the forklift 2 is finally estimated using the method, when the position estimation values of the two forklifts 2 are merged, the self-position of the forklift 2 is finally determined using, for example, a Kalman filter. It may be estimated.

  In the above embodiment, the first self-position estimator 3 for estimating the self-position of the forklift 2 using SLAM and the second self-position estimator 4 for estimating the self-position of the forklift 2 using GNSS are used. However, the present invention is not particularly limited to this mode, and three or more self-position estimators having different self-position estimation techniques may be used. In this case, the final self-position estimating unit 12 combines the three or more forklift 2 position estimation values obtained by the respective self-position estimators. Further, the self-position estimation technique to be used is not particularly limited to SLAM and GNSS.

  Further, in the above embodiment, the speed of the forklift 2 is measured by the wheel encoder 5 and the angular speed of the forklift 2 is measured by the gyro sensor 6, but the sensor used is not particularly limited thereto. For example, a sensor for measuring the speed of the forklift 2 may be a rotary encoder or the like, and a sensor for measuring the angular speed of the forklift 2 may be a steering angle sensor or a steering potentiometer. Further, the two-dimensional position and orientation of the forklift 2 may be directly detected by a sensor.

  Further, in the above embodiment, the position of the forklift 2 is estimated, but the present invention is also applicable to an autonomous mobile body such as an industrial vehicle or a mobile robot other than the forklift.

  DESCRIPTION OF SYMBOLS 1 ... Position estimation apparatus, 2 ... Forklift (autonomous mobile body), 3 ... 1st self-position estimator (self-position estimator), 4 ... 2nd self-position estimator (self-position estimator), 12 ... Final self-position Estimator.

Claims (4)

An autonomous mobile position estimating device that estimates the position of the autonomous mobile object,
A plurality of self-position estimators for estimating the self-position of the autonomous mobile body using different self-position estimation technology,
A final self-position estimating unit for finally estimating the self-position of the autonomous mobile based on the position estimation values of the plurality of autonomous mobiles obtained by the plurality of self-position estimators,
The final self-position estimating unit sets a weight coefficient of the plurality of self-position estimators based on the surrounding environment of the autonomous moving body, and uses the weight coefficients of the plurality of self-position estimators to calculate the weight of the autonomous moving body. An autonomous mobile position estimating device that ultimately estimates its own position.   The position estimation device for an autonomous moving body according to claim 1, wherein the final self-position estimating unit sets the weighting factor based on an environment of the self-position of the autonomous moving object that is last estimated last time. The self-position estimator estimates the two-dimensional position and orientation of the autonomous mobile as the self-position of the autonomous mobile,
The position estimation device for an autonomous mobile object according to claim 1, wherein the final self-position estimating unit sets a weight coefficient relating to a two-dimensional position and a direction of the autonomous mobile object as a weight coefficient of the self-position estimator. The number of the self-position estimators is two,
One of the two self-position estimators estimates the self-position of the autonomous mobile body using a self-position estimation technology in which the indoor estimation accuracy is higher than the outdoor estimation accuracy,
The other of the two self-position estimators estimates the self-position of the autonomous mobile body using a self-position estimation technique in which the estimation accuracy outdoors is higher than the estimation accuracy indoors. An autonomous mobile object position estimating apparatus according to claim 1.

Images