JP2021176052A - Self-position estimating device - Google Patents

Abstract

To provide a self-position estimating device that is able to prevent deviation of a self-position estimation value of a moving body from accumulating.SOLUTION: A self-position estimating device 16 includes: a laser sensor 3 that detects a distance from a moving body 2 to an object existing around the moving body; an odometry sensor 4 that detects an amount of movement of the moving body 2; a self-position estimating unit 10 that estimates a self-position of the moving body 2 by matching detection data of the laser sensor 3 with map data; a self-position interpolation unit 11 that acquires a self-position estimation value of the moving body 2 by interpolating the self-position of the moving body 2, based on the amount of movement of the moving body 2; a self-position deviation calculation unit 12 that calculates a difference between a first self-position estimation value of the moving body 2 and a second self-position estimation value of the moving body 2 as an amount of self-position deviation; an input device 5 that sets and inputs a reference position for the moving body 2 when estimation of the self-position of the moving body 2 is started; a reference position resetting unit 14 that resets a reference position when the amount of self-position deviation is equal to or larger than a threshold value B.SELECTED DRAWING: Figure 1

Description

The present invention relates to a self-position estimation device.

As a conventional self-position estimation device, for example, the technique described in Patent Document 1 is known. The self-position estimation device described in Patent Document 1 automatically irradiates the periphery of the automatic guided vehicle with an encoder that detects the amount of movement of the automatic guided vehicle and receives the laser reflected by a wall, a pillar, or the like. The external sensor that detects the environmental information around the automatic guided vehicle, the information input unit that inputs the route information such as the current location and destination of the automatic guided vehicle, and the detection information of the encoder and the external sensor and the input information of the information input unit. Based on this, it is equipped with a CPU that autonomously runs an automatic guided vehicle using the SLAM method that refers to map information and environmental information. The CPU calculates the first virtual value of the automatic guided vehicle based on the movement amount of the automatic guided vehicle, calculates a plurality of second virtual values of the automatic guided vehicle by the particle filter based on the specific environment information, and calculates the first virtual value. And a definite value is determined from each second virtual value, and the automatic guided vehicle is autonomously driven according to the definite value.

Japanese Unexamined Patent Publication No. 2018-13860

However, the above-mentioned prior art has the following problems. That is, for example, if the environment around the automatic guided vehicle, which is a moving body, changes and the environment information differs from the map information, the accuracy of estimating the self-position of the automatic guided vehicle decreases. If the automatic guided vehicle continues to run in that state, the deviation of the self-position estimated value of the automatic guided vehicle will be accumulated.

An object of the present invention is to provide a self-position estimation device capable of suppressing the accumulation of deviations of the self-position estimation values of a moving body.

One aspect of the present invention is a distance detection unit that detects the distance from a moving body to an object existing around the moving body in a self-position estimation device that estimates the self-position of the moving body when the moving body is traveling, and a moving body. It was detected by the movement amount detection unit that detects the movement amount of the body, the self-position estimation unit that estimates the self-position of the moving object by matching the detection data and the map data of the distance detection unit, and the movement amount detection unit. Based on the amount of movement of the moving body, the self-position interpolation unit that acquires the self-position estimation value of the moving body by interpolating the self-position of the moving body estimated by the self-position estimation unit, and the self-position estimation unit estimate The first self-position estimated value of the moving body obtained by interpolating the latest self-position of the moved body by the self-position interpolation unit and the previous self-position of the moving body are interpolated by the self-position interpolation unit. The self-position deviation calculation unit that calculates the difference from the acquired second self-position deviation value of the moving body as the self-position deviation amount, and the self-position deviation amount calculated by the self-position deviation calculation unit. The previous self-position determination unit that determines the previous self-position to be used next time in the calculation unit, and the setting input unit that sets and inputs the reference position of the moving body when starting the estimation of the self-position of the moving object by the self-position estimation unit. , The self-position deviation calculation unit is provided with a reference position resetting unit for resetting the reference position when the self-position deviation amount calculated by the self-position deviation calculation unit is equal to or higher than a predetermined first threshold value, and the self-position estimation unit is set. Set by the input unit Based on the input reference position, the estimation of the self-position of the moving body is started, and when the reference position is reset by the reference position resetting unit, it is based on the reset reference position. , Continue to estimate the self-position of the moving object.

In such a self-position estimation device, the distance detection unit detects the distance from the moving body to an object existing around the moving body, and the self-position of the moving body is estimated by matching the detected data with the map data. NS. Further, the movement amount detection unit detects the movement amount of the moving body, and the self-position of the moving body is interpolated based on the movement amount of the moving body, so that the self-position estimated value of the moving body is acquired. Then, the first self-position estimated value of the moving body obtained by interpolating the latest self-position of the moving body and the second self-position of the moving body obtained by interpolating the previous self-position of the moving body are interpolated. The difference from the estimated value is calculated as the amount of self-position deviation. Then, the previous self-position to be used next time is determined based on the self-position deviation amount. Here, the estimation of the self-position of the moving body is started based on the reference position set and input by the setting input unit. After the estimation of the self-position of the moving body is started, the reference position is reset when the self-position deviation amount becomes equal to or more than the first threshold value. Then, the self-position of the moving body is continuously estimated based on the reset reference position. Therefore, even if the accuracy of estimating the self-position of the moving body by matching the detected data and the map data tends to decrease, the estimated self-position of the moving body tends to approach the correct position. As a result, the accumulation of deviations of the self-position estimated values of the moving body is suppressed.

When the previous self-position deviation amount is equal to or less than the predetermined second threshold value, the previous self-position determination unit determines the first self-position estimated value as the previous self-position to be used next time, and the self-position deviation amount is the second. When it is larger than the threshold value, the second self-position estimated value may be determined as the previous self-position to be used next time. In such a configuration, an appropriate previous self-position can be obtained regardless of the error between the first self-position estimated value and the second self-position estimated value. Therefore, the accumulation of deviations of the self-position estimated values of the moving body is further suppressed.

The first threshold value may be smaller than the second threshold value. In such a configuration, if the error between the first self-position estimated value and the second self-position estimated value increases, the reference position will be reset at an early stage. Therefore, the accumulation of the estimated deviation of the self-position of the moving body is further suppressed.

When the self-position deviation amount is equal to or greater than the first threshold value, the reference position resetting unit may reset the reference position to a position corresponding to the second self-position estimated value or the second self-position estimated value. In such a configuration, the position corresponding to the second self-position estimated value or the second self-position estimated value obtained by interpolating the previous self-position of the moving body is used as the reference position, and therefore an appropriate reference. The position is easily obtained.

According to the present invention, it is possible to suppress the accumulation of deviations of the self-position estimated values of the moving body.

It is a block diagram which shows schematic the structure of the traveling control apparatus which includes the self-position estimation apparatus which concerns on one Embodiment of this invention. It is a conceptual diagram which shows the 1st self-position estimated value and the 2nd self-position estimated value. It is a flowchart which shows the procedure of the determination process executed by the self-position determination part last time shown in FIG. FIG. 3 is a conceptual diagram showing a threshold value A used in the determination process shown in FIG. 3 together with a first self-position estimated value and a second self-position estimated value. It is a flowchart which shows the procedure of the resetting process executed by the reference position resetting part shown in FIG. FIG. 5 is a conceptual diagram showing a threshold value B used in the resetting process shown in FIG. 5 together with a threshold value A, a first self-position estimated value, and a second self-position estimated value. It is a flowchart which shows the detail of the procedure of the self-position estimation process executed by the self-position estimation unit shown in FIG. The first self-position estimated value, the second self-position estimated value, and the state in which the previous self-position is determined with the passage of time in the state where the self-position estimation accuracy has not decreased and the state in which the self-position estimation accuracy has decreased. It is a conceptual diagram shown together with a true value. When the reference position of the moving body is reset in a state where the estimation accuracy of the self-position is lowered, the state in which the previous self-position is determined with the passage of time is the first self-position estimated value, the second self-position estimated value, and true. It is a conceptual diagram shown together with a value. It is a flowchart which shows the modification of the procedure of the determination process shown in FIG.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

FIG. 1 is a block diagram schematically showing a configuration of a travel control device including a self-position estimation device according to an embodiment of the present invention. In FIG. 1, the travel control device 1 is a device that automatically travels a moving body 2 (see FIG. 2) such as a forklift to a destination. The travel control device 1 is mounted on the moving body 2.

The travel control device 1 includes a laser sensor 3, an odometry sensor 4, an input device 5, a drive unit 6, and a controller 7.

The laser sensor 3 is a distance detection unit that detects the distance from the moving body 2 to an object existing around the moving body 2 by using a laser beam. The laser sensor 3 irradiates the periphery of the moving body 2 with a laser beam and receives the reflected light of the laser beam to detect the distance to an object existing around the moving body 2. Objects are walls, pillars, etc., and are registered in map data (described later). As the laser sensor 3, for example, a laser range finder is used. The laser beam emitted from the laser sensor 3 may be a 2D laser or a 3D laser.

The odometry sensor 4 is a movement amount detection unit that detects the movement amount of the moving body 2. As the odometry sensor 4, for example, an encoder or the like that detects the amount of movement of the moving body 2 by measuring the rotation angle of the wheels of the moving body 2 is used.

The input device 5 is a device for the user to input various information including the reference position of the moving body 2. The reference position here is the initial position of the moving body 2 when the self-position estimation unit 10 described later starts estimating the self-position of the moving body 2. The reference position is represented by two-dimensional coordinates (XY coordinates) and orientation. The user directly inputs the two-dimensional coordinates and the direction of the reference position by the input device 5. The input device 5 constitutes a setting input unit for setting and inputting a reference position of the moving body 2.

Although not particularly shown, the drive unit 6 includes a traveling motor that rotates the wheels of the moving body 2 and a steering motor that steers the wheels of the moving body 2.

The controller 7 is composed of a CPU, RAM, ROM, an input / output interface, and the like. The controller 7 includes a self-position estimation unit 10, a self-position interpolation unit 11, a self-position deviation calculation unit 12, a previous self-position determination unit 13, a reference position resetting unit 14, and a drive control unit 15. ing.

Here, the laser sensor 3, the odometry sensor 4, the input device 5, the self-position estimation unit 10, the self-position interpolation unit 11, the self-position deviation calculation unit 12, the previous self-position determination unit 13, and the reference position resetting unit 14 are The self-position estimation device 16 of the embodiment is configured. The self-position estimation device 16 is a device that estimates the self-position of the moving body 2 when the moving body 2 is traveling.

The self-position estimation unit 10 estimates the self-position of the moving body 2 by using the detection data of the laser sensor 3 and the map data of the surrounding environment of the moving body 2. The self-position estimation unit 10 estimates the self-position of the moving body 2 by using a laser SLAM (simultaneous localization and mapping) method. SLAM is a self-position estimation technology that performs self-position estimation using sensor data and map data. SLAM uses sensor data to estimate its own position and create an environmental map at the same time.

Specifically, the self-position estimation unit 10 matches the detection data of the laser sensor 3 with the map data of the surrounding environment of the moving body 2 and performs the self-position estimation calculation of the moving body 2. At this time, the self-position estimation unit 10 starts estimating the self-position of the moving body 2 based on the initial position (reference position) set and input by the input device 5. The self-position of the moving body 2 is represented by two-dimensional coordinates and orientation.

Since it takes time for the self-position estimation unit 10 to estimate the self-position of the moving body 2, the moving body 2 moves even while the self-position estimation calculation of the moving body 2 is being performed. Therefore, it is necessary to interpolate the distance traveled by the moving body 2 while the self-position estimating unit 10 is performing the calculation of estimating the self-position of the moving body 2.

The self-position interpolation unit 11 interpolates the self-position of the mobile body 2 estimated by the self-position estimation unit 10 based on the movement amount of the mobile body 2 detected by the odometry sensor 4, and thereby self of the mobile body 2. Get the position estimate. Specifically, the self-position interpolation unit 11 adds the movement amount of the moving body 2 detected by the odometry sensor 4 to the self-position of the moving body 2 estimated by the self-position estimation unit 10, so that the moving body 2 moves. Interpolate the self-position of 2 (see equations (A) and (B) below).

The self-position deviation calculation unit 12 calculates the difference between the first self-position estimated value of the moving body 2 and the second self-position estimated value of the moving body 2 as the self-position deviation amount.

The first self-position estimation value is a self-position estimation value obtained by interpolating the latest self-position of the moving body 2 estimated most recently by the self-position estimation unit 10 by the self-position interpolation unit 11. Specifically, in the first self-position estimated value, the latest self-position of the moving body 2 is interpolated by the amount of movement that the moving body 2 has advanced while the estimation calculation of the latest self-position of the moving body 2 is being performed. Is obtained by.

The second self-position estimated value is a self-position estimated value obtained by interpolating the previous self-position of the moving body 2 by the self-position interpolation unit 11. The previous self-position is acquired by interpolating the self-position of the moving body 2 previously estimated by the self-position estimation unit 10 by the self-position interpolation unit 11. Specifically, the second self-position estimation value is obtained by the moving body 2 while the latest self-position estimation calculation of the moving body 2 is being performed since the previous self-position estimation calculation of the moving body 2 was performed. It is acquired by interpolating the previous self-position of the moving body 2 by the amount of advanced movement.

For example, as shown in FIG. 2, the latest self-position of the estimated mobile 2 to the current period (the most recent) and x ₁ by the self position estimating unit 10 at time t1, the moving body 2 detected by the odometry sensor 4 Assuming that the amount of movement is Δ x ₁₂ , the first self-position estimated value x ₂ a at time t2 is expressed by the following equation.
x ₂ a = x ₁ + Δx ₁₂ ... (A)

On the other hand, the previous self-position of the moving body 2 at time t0 and x _0, when the movement amount of the moving object 2 detected from 4 to odometry sensor and [Delta] x _02, second self-position estimation value x _{2 b} at time t2, It is expressed by the following formula. The previous self-position x ₀ is a self-position obtained by interpolating the self-position of the moving body 2 estimated in the previous cycle (before the latest) by the self-position estimation unit 10 by the self-position interpolation unit 11. ..
x ₂ b = x ₀ + Δx ₀₂ ... (B)

Then, the self-position deviation amount d is represented by an absolute value of the following equation.
d = x ₂ a-x ₂ b ... (C)

The previous self-positioning unit 13 determines the previous self-position to be used next time by the self-positioning calculation unit 12 based on the self-positioning deviation amount d calculated by the self-positioning deviation calculation unit 12.

FIG. 3 is a flowchart showing the procedure of the determination process executed by the self-position determination unit 13 last time. In FIG. 3, the previous self-positioning unit 13 first acquires the self-positioning deviation amount d calculated by the self-positioning deviation calculation unit 12 (procedure S101).

Subsequently, the previous self-position determination unit 13 determines whether or not the self-position deviation amount d is equal to or less than the threshold value A (procedure S102). The threshold value A is a predetermined second threshold value.

When the previous self-position determination unit 13 determines that the self-position deviation amount d is equal to or less than the threshold value A, the previous self-position determination unit 13 determines the first self-position estimated value to the previous self-position (procedure S103). Therefore, as shown in FIG. 4A, when the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is equal to or less than the threshold value A, the first self It is determined that the position estimated value P is appropriate, and the previous self-position is updated to the first self-position estimated value P.

When the previous self-position determination unit 13 determines that the self-position deviation amount d is larger than the threshold value A, the previous self-position determination unit 13 determines the second self-position estimated value to the previous self-position (procedure S104). Therefore, as shown in FIG. 4B, when the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is larger than the threshold value A, the first self It is determined that the position estimation value P is not valid. Therefore, the previous self-position is not updated and remains as the second self-position estimated value Q.

The reference position resetting unit 14 resets the reference position of the moving body 2 when the self-positioning deviation amount d calculated by the self-positioning deviation calculation unit 12 is equal to or greater than the threshold value B. That is, the reference position resetting unit 14 resets the self-position estimation process of the moving body 2 by the self-position estimation unit 10.

FIG. 5 is a flowchart showing a procedure of the resetting process executed by the reference position resetting unit 14. In FIG. 5, the reference position resetting unit 14 first acquires the self-position deviation amount d calculated by the self-position deviation calculation unit 12 (procedure S111).

Subsequently, the reference position resetting unit 14 determines whether or not the self-position deviation amount d is equal to or greater than the threshold value B (procedure S112). The threshold value B is a predetermined first threshold value. As shown in FIG. 6, the threshold value B is a value smaller than the threshold value A.

When the reference position resetting unit 14 determines that the self-position deviation amount d is equal to or greater than the threshold value B, the reference position resetting unit 14 resets the reference position of the moving body 2 to the second self-position estimated value (procedure S113). When the reference position resetting unit 14 determines that the self-position deviation amount d is smaller than the threshold value B, the reference position resetting unit 14 does not execute the procedure S113.

Therefore, as shown in FIG. 6A, when the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is smaller than the threshold value B, the reference position No resetting process is performed. On the other hand, as shown in FIG. 6B, when the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is equal to or greater than the threshold value B, the reference position is set. The resetting process is executed.

The drive control unit 15 controls the drive unit 6 so as to move the moving body 2 toward the destination based on the self-position of the moving body 2 after interpolation obtained by the self-position interpolation unit 11.

The self-position estimation unit 10 starts estimating the self-position of the moving body 2 based on the reference position set and input by the input device 5, and the reference position is reset by the reference position resetting unit 14. At that time, the self-position of the moving body 2 is continuously estimated based on the reset reference position.

FIG. 7 is a flowchart showing the details of the procedure of the self-position estimation process executed by the self-position estimation unit 10. In FIG. 7, the self-position estimation unit 10 first determines whether or not the initial position (reference position) of the moving body 2 has been set and input by the input device 5 (procedure S121).

When the self-position estimation unit 10 determines that the initial position of the moving body 2 has been set and input by the input device 5, the self-position estimation unit 10 starts the self-position estimation calculation of the moving body 2 described above based on the initial position (procedure S122). ).

Subsequently, the self-position estimation unit 10 determines whether or not the reference position of the moving body 2 has been reset by the reference position resetting unit 14 (procedure S123). When the self-position estimation unit 10 determines that the reference position has been reset by the reference position resetting unit 14, the self-position estimation unit 10 continues to estimate the self-position of the moving body 2 described above based on the new reference position reset. (Procedure S124). Then, the self-position estimation unit 10 executes the procedure S123 again.

When the self-position estimation unit 10 determines that the reference position has not been reset by the reference position resetting unit 14, the self-position of the moving body 2 described above is based on the current reference position that has not been reset. The estimation is continued (procedure S125). Then, the self-position estimation unit 10 executes the procedure S123 again.

In the self-position estimation device 16 as described above, as shown in FIG. 8A, the self-position which is the difference between the first self-position estimation value P and the second self-position estimation value Q obtained at time t0. When the deviation amount d is equal to or less than the threshold value A, the first self-position estimated value P is adopted as the reference position used in the next cycle (t1). Then, when the self-position deviation amount d between the first self-position estimated value P and the second self-position estimated value Q obtained at time t1 is also equal to or less than the threshold value A, the first self-position estimated value P is set to the next cycle ( It is adopted as the reference position used in t2). The diamond mark in FIG. 8 represents the true value R, which is the actual self-position of the moving body 2. However, in the processing by the controller 7, the true value R is unknown.

However, if, for example, the environment around the moving body 2 changes and the environment around the moving body 2 becomes different from the map data, the estimation accuracy of the self-position of the moving body 2 deteriorates. In this case, since the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is accumulated, an inappropriate reference position may be adopted.

For example, as shown in FIG. 8B, the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q obtained at time t1, is equal to or less than the threshold value A. Even if the first self-position estimated value P is adopted as the reference position used in the next cycle (t2), the first self-position estimated value P and the second self-position estimated value Q obtained at time t2 The self-positioning deviation amount d, which is a difference, may be larger than the threshold value A. In this case, although the first self-position estimated value P close to the true value R is obtained, the first self-position estimated value P is not adopted as the reference position. When the self-position deviation amount d is accumulated in this way, the self-position estimated value of the moving body 2 cannot be returned to the correct position.

However, in the present embodiment, as shown in FIG. 9, the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q obtained at time t0, is equal to or greater than the threshold value B. When is, the reference position of the moving body 2 is reset to the second self-position estimated value Q. Then, the estimation of the self-position of the moving body 2 is continuously performed based on the reset reference position.

Then, at time t1, the first self-position estimated value P close to the true value R is obtained, and the self-position deviation amount d, which is the difference between the first self-position estimated value P and the second self-position estimated value Q, is the threshold value B. Is smaller than Then, even at time t2, the first self-position estimated value P closer to the true value R is obtained, and the self-position deviation amount d between the first self-position estimated value P and the second self-position estimated value Q is from the threshold value B. Also becomes smaller. As a result, even if the estimation accuracy of the self-position of the moving body 2 deteriorates due to a change in the surrounding environment of the moving body 2, the self-position estimated value of the moving body 2 can be returned to the correct position.

As described above, according to the present embodiment, the distance from the moving body 2 to the object existing around the moving body 2 is detected by the laser sensor 3, and the self of the moving body 2 is matched with the detected data and the map data. The position is estimated. Further, the movement amount of the moving body 2 is detected by the odometry sensor 4, and the self-position of the moving body 2 is interpolated based on the moving amount of the moving body, so that the self-position estimated value of the moving body 2 is acquired. .. Then, the first self-position estimated value of the moving body 2 acquired by interpolating the latest self-position of the moving body 2 and the moving body 2 acquired by interpolating the previous self-position of the moving body 2 The difference from the second self-position estimated value is calculated as the self-position deviation amount. Then, the previous self-position to be used next time is determined based on the self-position deviation amount. Here, the estimation of the self-position of the moving body 2 is started based on the reference position set and input by the input device 5. After the estimation of the self-position of the moving body 2 is started, the reference position is reset when the self-position deviation amount becomes equal to or less than the threshold value B. Then, the self-position of the moving body 2 is continuously estimated based on the reset reference position. Therefore, even if the accuracy of estimating the self-position of the moving body 2 by matching the detected data and the map data tends to decrease, the estimated self-position of the moving body 2 tends to approach the correct position. As a result, the accumulation of deviations of the self-position estimated values of the moving body 2 is suppressed. As a result, the running of the moving body 2 can be continued with the estimation accuracy of the self-position of the moving body 2 being good.

Further, in the present embodiment, when the self-position deviation amount is equal to or less than the threshold value A, the first self-position estimated value is determined as the previous self-position to be used next time, and when the self-position deviation amount is larger than the threshold value A. , The second self-position estimate is determined as the previous self-position to be used next time. Therefore, an appropriate previous self-position can be obtained regardless of the error between the first self-position estimated value and the second self-position estimated value. Therefore, the accumulation of deviations of the self-position estimated values of the moving body 2 is further suppressed.

Further, in the present embodiment, since the threshold value B is smaller than the threshold value A, if the error between the first self-position estimated value and the second self-position estimated value increases, the reference position is reset at an early stage. .. Therefore, the accumulation of the estimated deviation of the self-position of the moving body 2 is further suppressed.

Further, in the present embodiment, when the self-position deviation amount is equal to or greater than the threshold value B, the reference position is reset to the second self-position estimated value. Since the second self-position estimated value obtained by interpolating the previous self-position of the moving body 2 in this way is used as the reference position, an appropriate reference position can be easily obtained.

The present invention is not limited to the above embodiment. For example, in the above embodiment, the threshold value B is smaller than the threshold value A, but the embodiment is not particularly limited, and the threshold value B may be equal to the threshold value A, or the threshold value B may be larger than the threshold value A.

Further, in the above embodiment, the initial position (reference position) of the moving body 2 is set and input by the input device 5, but the present invention is not particularly limited to such a form. For example, the controller 7 reads a storage unit that stores specific information such as a number associated with the initial position of the moving body 2 and an initial position corresponding to the specific information input by the input device 5 from the storage unit and sets the controller 7. It may have a setting unit. In this case, the input device 5, the storage unit, and the setting unit constitute the above-mentioned setting input unit.

Further, in the above embodiment, when the self-position deviation amount d, which is the difference between the first self-position estimated value P of the moving body 2 and the second self-position estimated value Q of the moving body 2, is equal to or less than the threshold value A, the first 1 When the self-position estimated value P is determined to be the previous self-position and the self-position deviation amount d is larger than the threshold value A, the second self-position estimated value Q is determined to be the previous self-position. Therefore, the previous self-position may be determined according to the reliability of the self-position estimation of the moving body 2.

FIG. 10 is a flowchart showing a modified example of the determination processing procedure shown in FIG. In FIG. 10, the previous self-position determination unit 13 determines whether or not the self-position estimation of the moving body 2 by the self-position estimation unit 10 is highly reliable after executing the above procedure S101 (procedure S107).

At this time, the previous self-position determination unit 13 calculates the dispersion values of the plurality of self-position candidate points of the moving body 2 based on the detection data of the laser sensor 3, for example. Then, when the variance value of the plurality of self-position candidate points is equal to or higher than a predetermined threshold value, the previous self-position determination unit 13 determines that the self-position accuracy of the moving body 2 is high, and determines that the self-position accuracy of the plurality of self-position candidate points is high. When the variance value of is lower than the threshold value, it is determined that the self-position accuracy of the moving body 2 is low.

When the self-positioning unit 13 determines that the self-position estimation of the moving body 2 has high reliability last time, the self-positioning unit 13 executes the above procedure S102. Therefore, when the reliability of the self-position estimation of the moving body 2 is high and the self-position deviation amount d is equal to or less than the threshold value A, the first self-position estimation value is determined to be the previous self-position. When the self-position determination unit 13 determines that the reliability of the self-position estimation of the moving body 2 is low last time, the self-position determination unit 13 executes the above procedure S104. Therefore, when the reliability of the self-position estimation of the moving body 2 is low, the second self-position estimation value is determined to be the previous self-position regardless of the self-position deviation amount d.

Further, in the above embodiment, the detection data of the laser sensor 3 and the map data are matched to perform the estimation calculation of the self-position of the moving body 2, and the detection data and the map data of such a laser sensor 3 are used. Based on the amount of movement of the moving body 2 detected by the odometry sensor 4, the self-position of the moving body 2 is probabilistically estimated using, for example, a time-series data prediction method called a particle filter. You may. In the particle filter, many next states that can occur from the current state are represented by a large number of particles, and the weighted average calculated according to the likelihood of all particles (likeness of the object to be tracked) is estimated to be the next state and tracked. I do.

Further, in the above embodiment, the reference position is reset to the second self-position estimated value in the reference position resetting unit 14, but the reference position is set to the second self-position estimated value without being particularly limited to that form. It may be reset to the corresponding position. For example, when estimating the self-position of the moving body 2 using the above particle filter, the self-position of the moving body 2 is recalculated by changing the number of particles based on the second self-position estimated value. The calculation result may be reset as the reference position.

Further, in the above embodiment, the movement amount of the moving body 2 is detected by the odometry sensor 4, but the movement amount is not particularly limited to the odometry sensor 4, and an inertial measurement unit for measuring the angular velocity and acceleration of the moving body 2 is used. The movement amount of the moving body 2 may be detected by using the measured value of the inertial measurement unit.

Further, in the above embodiment, the self-position of the moving body 2 is estimated by the laser SLAM using the detection data of the laser sensor 3, but the self-position is not particularly limited to the laser SLAM, and for example, the surroundings of the moving body 2 are imaged. The self-position of the moving body 2 may be estimated by the image SLAM using the image data of the camera.

2 ... moving body, 3 ... laser sensor (distance detection unit), 4 ... odometry sensor (movement amount detection unit), 5 ... input device (setting input unit), 10 ... self-position estimation unit, 11 ... self-position interpolation unit, 12 ... Self-position deviation calculation unit, 13 ... Previous self-position determination unit, 14 ... Reference position resetting unit, 16 ... Self-position estimation device, A ... Threshold (second threshold), B ... Threshold (first threshold), P ... 1st self-position estimated value, Q ... 2nd self-position estimated value.

Claims (4)

In a self-position estimation device that estimates the self-position of the moving body when the moving body is traveling,
A distance detection unit that detects the distance from the moving body to an object existing around the moving body, and
A movement amount detection unit that detects the movement amount of the moving body,
A self-position estimation unit that estimates the self-position of the moving body by matching the detection data of the distance detection unit with the map data,
The self-position estimated value of the moving body is acquired by interpolating the self-position of the moving body estimated by the self-position estimation unit based on the moving amount of the moving body detected by the moving amount detecting unit. Self-position interpolation section and
The first self-position estimated value of the moving body obtained by interpolating the latest self-position of the moving body estimated by the self-position estimation unit by the self-position interpolation unit, and the previous self-position of the moving body. The self-position deviation calculation unit that calculates the difference from the second self-position estimated value of the moving body obtained by interpolating by the self-position interpolation unit as the self-position deviation amount.
Based on the self-position deviation amount calculated by the self-position deviation calculation unit, the previous self-position determination unit that determines the previous self-position to be used next time in the self-position deviation calculation unit, and the previous self-position determination unit.
A setting input unit for setting and inputting a reference position of the moving body when the self-position estimation unit starts estimating the self-position of the moving body.
A reference position resetting unit for resetting the reference position when the self-positioning deviation amount calculated by the self-positioning deviation calculation unit is equal to or greater than a predetermined first threshold value is provided.
The self-position estimation unit starts estimating the self-position of the moving body based on the reference position set and input by the setting input unit, and the reference position is reset by the reference position resetting unit. When, a self-position estimation device that continuously estimates the self-position of the moving body based on the reset reference position. When the self-position deviation amount is equal to or less than a predetermined second threshold value, the previous self-position determination unit determines the first self-position estimated value as the previous self-position to be used next time, and determines the self-position as the previous self-position to be used next time. The self-position estimation device according to claim 1, wherein when the deviation amount is larger than the second threshold value, the second self-position estimation value is determined as the previous self-position to be used next time. The self-position estimation device according to claim 2, wherein the first threshold value is smaller than the second threshold value. When the self-position deviation amount is equal to or greater than the first threshold value, the reference position resetting unit resets the reference position to a position corresponding to the second self-position estimated value or the second self-position estimated value. The self-position estimation device according to any one of claims 1 to 3.

Images