JP2021026372A - Position estimation device - Google Patents

Abstract

To provide a position estimation device capable of estimating a self position of a moving body with high accuracy.SOLUTION: A position estimation device 15 comprises: a laser sensor 3 detecting a distance to an object 7 around a moving body 2 by irradiating surroundings of the moving body 2 with a laser and receiving reflected light of the laser; an estimation operation unit 11 performing an estimation operation of a self position of the moving body 2 based on the detected data of the laser sensor 3; and an estimation accuracy detection unit 12 detecting estimation accuracy of the self position of the moving body 2 by the estimation operation unit 11. The estimation operation unit 11 changes an usage distance range of the detected data of the laser sensor 3 according to the estimation accuracy of the self position of the moving body 2 detected by the estimation accuracy detection unit 12 and performs the estimation operation of the self position of the moving body 2.SELECTED DRAWING: Figure 1

Description

The present invention relates to a position estimation device.

As a conventional position estimation device, for example, a technique as described in Patent Document 1 is known. The position estimation device described in Patent Document 1 sets a laser distance sensor that emits a laser, detects the reflected light of the laser, and measures the distance to an object, and an address and a traveling area of an automatic guided vehicle. It is equipped with a data memory that stores correspondence information with the coordinates and a processing unit that estimates the current position of the automatic guided vehicle by matching the measurement data from the laser distance sensor with the map data.

Japanese Unexamined Patent Publication No. 2011-253414

However, when estimating the self-position of a moving body using a laser as in the above-mentioned conventional technique, if the number of objects reflecting the laser is small, the accuracy of estimating the self-position of the moving body is lowered. ..

An object of the present invention is to provide a position estimation device capable of estimating the self-position of a moving body with high accuracy.

One aspect of the present invention is a position estimation device that estimates the self-position of a moving body by irradiating the periphery of the moving body with a laser and receiving the reflected light of the laser to reduce the distance to an object around the moving body. An estimation accuracy detection that detects the estimation accuracy of the self-position of the moving body by the estimation calculation unit and the estimation calculation unit that performs the estimation calculation of the self-position of the moving body based on the detection data of the distance detection unit and the distance detection unit. The estimation calculation unit is provided with a unit, and the estimation calculation unit changes the usage distance range of the detection data of the distance detection unit according to the estimation accuracy of the self-position of the moving body detected by the estimation accuracy detection unit, and determines the self-position of the moving body. Performs an estimation operation.

In such a position estimation device, the distance detection unit irradiates the periphery of the moving body with a laser to detect the distance to an object around the moving body. Then, the estimation calculation unit performs an estimation calculation of the self-position of the moving body based on the detection data of the distance detection unit. Here, the estimation accuracy detection unit detects the estimation accuracy of the self-position of the moving body by the estimation calculation unit. Then, the estimation calculation unit changes the used distance range of the detection data of the distance detection unit according to the estimation accuracy of the self-position of the moving body, and performs the estimation calculation of the self-position of the moving body. At this time, the lower the estimation accuracy of the self-position of the moving body, the wider the usable distance range of the detection data of the distance detection unit. When the usable distance range of the detection data of the distance detection unit is widened, the number of objects reflecting the laser within the usable distance range of the detected data tends to increase. As a result, the self-position of the moving body can be estimated with high accuracy.

When the estimation accuracy of the self-position of the moving body is low, the estimation calculation unit widens the range of use of the detection data of the distance detection unit as compared with the case where the estimation accuracy of the self-position of the moving body is high. The self-position estimation calculation may be performed. In such a configuration, when the estimation accuracy of the self-position of the moving object is low, the usable distance range of the detection data of the distance detection unit becomes wide, so that the number of objects that reflect the laser within the usable distance range of the detected data is large. It will definitely increase easily.

The estimation accuracy detection unit detects whether or not the estimation accuracy of the self-position of the moving object by the estimation calculation unit has decreased, and the estimation calculation unit detects whether the estimation accuracy of the self-position of the moving object has decreased, and the estimation calculation unit determines the distance when the estimation accuracy of the self-position of the moving object has not decreased. Using the first distance range as the usage distance range of the detection data of the detection unit, the self-position estimation calculation of the moving body is performed, and when the estimation accuracy of the self-position of the moving body is low, the distance detection unit The self-position estimation calculation of the moving body may be performed by using the second distance range wider than the first distance range as the usage distance range of the detection data. In such a configuration, only two ranges, a first distance range and a second distance range, are used as the usable distance range of the detection data of the distance detection unit. Therefore, it is possible to simplify the arithmetic processing executed by the estimation arithmetic unit.

The position estimation device further includes a mark detection unit that detects a mark installed on the road surface on which the moving body moves, and the estimation calculation unit performs an estimation calculation of the self-position of the moving body using the first distance range. Then, the self-position of the moving body is estimated as the first position estimated value, and the estimation accuracy detection unit estimates the self-position of the moving body as the second position estimated value based on the mark detected by the mark detecting unit. When the amount of deviation between the first position estimated value and the second position estimated value is equal to or greater than the threshold value, it may be detected that the estimation accuracy of the self-position of the moving body is lowered. In such a configuration, the first position estimated value obtained based on the detection data of the distance detection unit is compared with the second position estimated value obtained based on the mark installed on the road surface to move. It is detected whether or not the estimation accuracy of the self-position of the body is reduced. Therefore, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body is lowered.

The position estimation device further includes a state detection unit that detects the state of the moving body, and the estimation calculation unit calculates the self-position of the moving body by using the first distance range to estimate the self-position of the moving body. Is estimated as the first position estimated value, and the estimation accuracy detection unit estimates the self-position of the moving body as the second position estimated value based on the state of the moving body detected by the state detection unit, and estimates the first position. When the amount of deviation between the value and the second position estimated value is equal to or greater than the threshold value, it may be detected that the estimation accuracy of the self-position of the moving body is lowered. In such a configuration, the first position estimated value obtained based on the detection data of the distance detection unit and the second position estimated value obtained based on the state of the moving body are compared with each other to obtain the moving body. It is detected whether or not the estimation accuracy of the self-position is lowered. Therefore, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body is lowered.

When the amount of deviation between the first position estimated value and the second position estimated value is continuously equal to or greater than the threshold value for a specified time or a specified distance, the estimation accuracy detection unit reduces the estimation accuracy of the self-position of the moving body. It may be detected as being present. In such a configuration, the state in which the amount of deviation between the first position estimated value and the second position estimated value momentarily exceeds the threshold value is excluded, so whether or not the estimation accuracy of the self-position of the moving body is lowered. Can be detected more accurately.

The estimation accuracy detection unit calculates the variance value of a plurality of self-position candidate points of the moving body based on the detection data of the distance detection unit, and when the variance value is equal to or more than the threshold value, the estimation accuracy of the self-position of the moving body is estimated. May be detected as decreasing. In such a configuration, it is detected whether or not the estimation accuracy of the self-position of the moving body is lowered by the calculation process based on the detection data of the distance detection unit. Therefore, with a simple configuration, it is possible to detect whether or not the estimation accuracy of the self-position of the moving body is lowered.

The estimation accuracy detection unit may detect that the estimation accuracy of the self-position of the moving body is lowered when the dispersion value is continuously equal to or higher than the threshold value for a specified time or a specified distance. In such a configuration, since the state in which the dispersion value momentarily exceeds the threshold value is excluded, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body is lowered.

According to the present invention, the self-position of the moving body can be estimated with high accuracy.

It is a schematic block diagram which shows the traveling control apparatus provided with the position estimation apparatus which concerns on 1st Embodiment of this invention. It is a conceptual diagram which shows the irradiation range of the laser by the laser sensor shown in FIG. 1 together with the AR marker installed on the road surface. It is a flowchart which shows the detail of the arithmetic processing procedure executed by the estimation arithmetic unit shown in FIG. It is a conceptual diagram which shows a plurality of self-position candidate points of a moving body created when the self-position estimation calculation of a moving body is performed by the estimation calculation unit shown in FIG. It is a flowchart which shows the detail of the detection processing procedure executed by the estimation accuracy detection unit shown in FIG. It is a conceptual diagram which shows how the estimation calculation unit shown in FIG. 1 performs the estimation calculation of the self-position of a moving body using the normal distance range or the long-distance range as the use distance range of the detection data of a laser sensor. It is a schematic block diagram which shows the traveling control apparatus provided with the position estimation apparatus which concerns on 2nd Embodiment of this invention. FIG. 5 is a flowchart showing details of a detection processing procedure executed by the estimation accuracy detection unit shown in FIG. 7. It is a schematic block diagram which shows the traveling control apparatus provided with the position estimation apparatus which concerns on 3rd Embodiment of this invention. 9 is a flowchart showing details of a detection processing procedure executed by the estimation accuracy detection unit shown in FIG. 9. It is a model diagram which shows the state of measuring the degree of matching between the self-position candidate point of a moving body and a map. It is a conceptual diagram which shows the state that there are many self-position candidate points having a high degree of matching with a map as self-position candidate points of a moving body. It is a conceptual diagram which shows the state that there are many self-position candidate points having a low degree of matching with a map as self-position candidate points of a moving body.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the drawings, the same or equivalent elements are designated by the same reference numerals, and duplicate description will be omitted.

FIG. 1 is a schematic configuration diagram showing a travel control device including a position estimation device according to the first 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 or an automatic guided vehicle to a destination along a travel route. The travel control device 1 is mounted on the moving body 2.

The travel control device 1 includes a laser sensor 3, a camera 4, a drive unit 5, and a controller 6.

The laser sensor 3 constitutes a distance detection unit that detects the distance to an object around the moving body 2 by irradiating the surroundings of the moving body 2 with a laser and receiving the reflected light of the laser. As the laser sensor 3, for example, a laser range finder is used. The laser to be used may be a 2D laser or a 3D laser.

As shown in FIG. 2, the laser sensor 3 irradiates the laser L in a fan shape. Specifically, the laser sensor 3 irradiates the laser L to a predetermined angle range (here, 270 degrees) centered on the forward straight direction of the moving body 2. The laser L emitted from the laser sensor 3 hits an object 7 (see FIG. 6), and the laser (reflected light) reflected by the object 7 is received by the laser sensor 3. The object 7 is a stationary object such as a wall or a pillar, and is registered in the map data.

As shown in FIG. 2, the camera 4 captures an AR (augmented reality) marker 8 which is a mark installed on the road surface on which the moving body 2 moves. The camera 4 constitutes a mark detection unit that detects the AR marker 8.

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

The controller 6 is composed of a CPU, RAM, ROM, an input / output interface, and the like. The controller 6 has an estimation calculation unit 11, an estimation accuracy detection unit 12, and a drive control unit 13.

The estimation calculation unit 11 calculates the self-position of the moving body 2 based on the detection data of the laser sensor 3. The estimation calculation unit 11 performs an estimation calculation of the self-position of the moving body 2 by using, for example, a 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 a laser sensor or the like to estimate its own position and create an environmental map at the same time.

Specifically, the estimation calculation unit 11 matches the distance data to the object 7 detected by the laser sensor 3 with the map data of the surrounding environment of the moving body 2 to perform an estimation calculation of the self-position of the moving body 2. Do. At this time, the estimation calculation unit 11 changes the usage distance range of the detection data of the laser sensor 3 according to the estimation accuracy of the self-position of the moving body 2 detected by the estimation accuracy detecting unit 12 described later, and the moving body 2 Performs an estimation operation of the self-position of. More specifically, when the estimation accuracy of the self-position of the moving body 2 detected by the estimation accuracy detection unit 12 is low, the estimation calculation unit 11 determines the self-position of the moving body 2 detected by the estimation accuracy detection unit 12. The range of use distance of the detection data of the laser sensor 3 is widened as compared with the case where the estimation accuracy is high, and the self-position estimation calculation of the moving body 2 is performed. The specific processing of the estimation calculation unit 11 will be described in detail later.

The estimation accuracy detection unit 12 detects the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 based on the image data of the AR marker 8 captured by the camera 4. Specifically, the estimation accuracy detection unit 12 detects whether or not the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered based on the image data of the AR marker 8. The specific processing of the estimation accuracy detection unit 12 will be described in detail later.

The drive control unit 13 controls the drive unit 5 so that the moving body 2 travels along the traveling path to the destination based on the self-position of the moving body 2 estimated by the estimation calculation unit 11.

The position estimation device 15 of the present embodiment includes a laser sensor 3, a camera 4, an estimation calculation unit 11 of the controller 6, and an estimation accuracy detection unit 12.

FIG. 3 is a flowchart showing details of a calculation processing procedure executed by the estimation calculation unit 11. This process is executed when the start of traveling of the moving body 2 is instructed.

In FIG. 3, the estimation calculation unit 11 first acquires the detection data of the laser sensor 3 (procedure S101). Subsequently, the estimation calculation unit 11 uses the normal distance range P (see FIGS. 6A and 6B) as the usage distance range of the detection data of the laser sensor 3, and estimates the self-position of the moving body 2. (Procedure S102).

The normal distance range P is a first distance range used when the moving body 2 travels in a place where the object 7 required for the self-position estimation calculation exists in the vicinity of the traveling path of the moving body 2. The normal distance range P is preset.

The estimation calculation unit 11 is a laser sensor which is a laser which is a laser which is a laser which is irradiated from the laser sensor 3 and is reflected by hitting an object 7, and is a laser which is reflected by hitting an object 7 arranged within a normal distance range P among the detection data of the laser sensor 3. The self-position estimation calculation of the moving body 2 is performed using only the detection data of 3.

Specifically, as shown in FIG. 4, the estimation calculation unit 11 has distance data to the object 7 around the moving body 2 and the surroundings of the moving body 2 within the normal distance range P of the detection data of the laser sensor 3. By matching with the map data of the environment, a plurality of self-position candidate points S of the moving body 2 are created. The estimation calculation unit 11 calculates the weight of each self-position candidate point S by calculating the degree of matching with the map data when it is assumed that the laser is irradiated from each self-position candidate point S. Then, the estimation calculation unit 11 sets the weighted average value of the plurality of self-position candidate points S having weights at the self-position of the moving body 2. At this time, the estimation calculation unit 11 estimates the self-position of the moving body 2 as the first position estimation value. The self-position of the moving body 2 is represented by two-dimensional coordinates (XY coordinates) and orientation.

Subsequently, the estimation calculation unit 11 outputs the first position estimation value to the estimation accuracy detection unit 12 (procedure S103). Subsequently, the estimation calculation unit 11 determines whether or not the estimation accuracy detection signal (described later) from the estimation accuracy detection unit 12 has been input (procedure S104).

When the estimation calculation unit 11 determines that the estimation accuracy detection signal has been input, the estimation calculation unit 11 acquires the detection data of the laser sensor 9 (procedure S105). Then, the estimation calculation unit 11 determines whether or not the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered based on the estimation accuracy detection signal (procedure S106).

When the estimation calculation unit 11 determines that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 has not deteriorated, the estimation calculation unit 11 sets the usage distance range of the detection data of the laser sensor 3 as in the above procedure S102. The self-position estimation calculation of the moving body 2 is performed using the normal distance range P (procedure S107).

When the estimation calculation unit 11 determines that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is low, the estimation calculation unit 11 determines that the usage distance range of the detection data of the laser sensor 3 is a long distance range Q (FIG. 6 (FIG. 6). c)) is used to estimate the self-position of the moving body 2 (procedure S108).

The long-distance range Q is a second distance range used when the moving body 2 travels in a place where the object 7 required for the self-position estimation calculation is insufficient in the vicinity of the traveling path of the moving body 2. The long-distance range Q is a working distance range wider than the normal distance range P. The long-distance range Q is preset so as to satisfy the number of objects 7 required for the self-position estimation calculation.

The estimation calculation unit 11 is a laser sensor which is a laser which is a laser which is a laser which is irradiated from the laser sensor 3 and is reflected by hitting an object 7 and is a laser which is reflected by hitting an object 7 arranged in a long distance range Q among the detection data of the laser sensor 3. The self-position estimation calculation of the moving body 2 is performed using only the detection data of 3.

Specifically, the estimation calculation unit 11 matches the distance data to the object 7 around the moving body 2 in the long distance range Q of the detection data of the laser sensor 3 with the map data of the surrounding environment of the moving body 2. As a result, a plurality of self-position candidate points S of the moving body 2 are created. The estimation calculation unit 11 calculates the weight of each self-position candidate point S by calculating the degree of matching with the map data when it is assumed that the laser is irradiated from each self-position candidate point S. Then, the estimation calculation unit 11 sets the weighted average value of the plurality of self-position candidate points S having weights at the self-position of the moving body 2.

Subsequently, the estimation calculation unit 11 outputs the self-position of the moving body 2 obtained in the procedure S107 or the procedure S108 to the drive control unit 13 (procedure S109). After that, the estimation calculation unit 11 executes the above procedure S101 again.

When the estimation calculation unit 11 determines in the procedure S106 that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 has not deteriorated, the estimation calculation unit 11 obtained the estimation calculation unit 11 in the procedure S102 without executing the procedure S107. The self-position of the moving body 2 may be output to the drive control unit 13.

FIG. 5 is a flowchart showing details of the detection processing procedure executed by the estimation accuracy detection unit 12. Similar to the estimation calculation unit 11, this process is also executed when the start of traveling of the moving body 2 is instructed.

In FIG. 5, the estimation accuracy detection unit 12 first acquires the image data of the AR marker 8 captured by the camera 4 (procedure S111). Then, the estimation accuracy detection unit 12 estimates the self-position of the moving body 2 as the second position estimation value based on the image data of the AR marker 8 (procedure S112).

Subsequently, the estimation accuracy detection unit 12 determines whether or not the first position estimated value from the estimation calculation unit 11 has been input (procedure S113). When the estimation accuracy detection unit 12 determines that the first position estimated value has been input, the estimation accuracy detection unit 12 calculates the amount of deviation between the first position estimated value and the second position estimated value (procedure S114).

Then, the estimation accuracy detection unit 12 determines whether or not the amount of deviation between the first position estimated value and the second position estimated value is equal to or greater than a predetermined threshold value (procedure S115). The threshold values are individually set for the amount of deviation of the X coordinate, the amount of deviation of the Y coordinate, and the amount of deviation of the direction between the first position estimated value and the second position estimated value.

When the estimation accuracy detection unit 12 determines that the amount of deviation between the first position estimated value and the second position estimated value is equal to or greater than the threshold value, the amount of deviation between the first position estimated value and the second position estimated value is the threshold value. It is determined whether or not the above-mentioned states are continuous for a predetermined predetermined number of times (specified cycle) (procedure S116). That is, the estimation accuracy detection unit 12 determines whether or not the amount of deviation between the first position estimated value and the second position estimated value is continuously equal to or greater than the threshold value for a predetermined time. At this time, in the estimation accuracy detection unit 12, for example, at least one of the X-coordinate deviation amount, the Y-coordinate deviation amount, and the orientation deviation amount between the first position estimated value and the second position estimated value is equal to or greater than the corresponding threshold value. If, the procedure S116 is executed.

When the estimation accuracy detection unit 12 determines that the state in which the deviation amount between the first position estimated value and the second position estimated value is equal to or greater than the threshold value is continuous for a predetermined number of times, the moving body 2 by the estimation calculation unit 11 It is determined that the estimation accuracy of the self-position is low (procedure S117). At this time, the estimation accuracy detection unit 12 determines that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lower than the predetermined reference value.

When the estimation accuracy detection unit 12 determines in step S115 that the amount of deviation between the first position estimated value and the second position estimated value is not equal to or greater than the threshold value, or in step S116, the first position estimated value and the second position estimated value are When it is determined that the state in which the deviation amount is equal to or greater than the threshold value is not continuous a specified number of times, it is determined that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 has not deteriorated (procedure S118). At this time, the estimation accuracy detection unit 12 determines that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is equal to or higher than the reference value.

Subsequently, the estimation accuracy detection unit 12 outputs the result determined in the procedure S117 or the procedure S118 to the estimation calculation unit 11 as an estimation accuracy detection signal (procedure S119). After that, the estimation accuracy detection unit 12 executes the above procedure S111 again.

In the traveling control device 1 configured as described above, when the moving body 2 is traveling, the distance to the object 7 around the moving body 2 is detected by irradiating the periphery of the moving body 2 with a laser by the laser sensor 3. Will be done. Then, the estimation calculation unit 11 of the controller 6 performs an estimation calculation of the self-position of the moving body 2 based on the detection data of the laser sensor 3. Then, the drive control unit 13 of the controller 6 controls the moving body 2 to travel toward the destination based on the self-position of the moving body 2.

Here, normally, as shown in FIG. 6A, the position estimation calculation of the moving body 2 is performed using the normal distance range P as the used distance range of the detection data of the laser sensor 3. At this time, when a large number of objects 7 exist within the normal distance range P of the detection data of the laser sensor 3, the objects 7 are easily caught by the laser irradiated from the laser sensor 3, so that the self-position of the moving body 2 Is estimated correctly.

However, as shown in FIG. 6B, when the number of objects 7 existing in the normal distance range P of the detection data of the laser sensor 3 is small, the objects 7 are included in the laser emitted from the laser sensor 3. Since it is difficult to catch, the estimation accuracy of the self-position of the moving body 2 is lowered.

Therefore, the estimation accuracy detection unit 12 of the controller 6 detects whether or not the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered. Then, when it is detected that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered, as shown in FIG. 6 (c), it is set as the working distance range of the detection data of the laser sensor 3. The self-position estimation calculation of the moving body 2 is performed using the long-distance range Q. Then, a large number of objects 7 will exist within the long-distance range Q of the detection data of the laser sensor 3. Therefore, since the object 7 is easily captured by the laser emitted from the laser sensor 3, the self-position of the moving body 2 is correctly estimated.

As described above, in the present embodiment, the distance to the object 7 around the moving body 2 is detected by irradiating the periphery of the moving body 2 with the laser by the laser sensor 3. Then, the estimation calculation unit 11 performs an estimation calculation of the self-position of the moving body 2 based on the detection data of the laser sensor 3. Here, the estimation accuracy detection unit 12 detects the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11. Then, the estimation calculation unit 11 changes the usage distance range of the detection data of the laser sensor 3 according to the estimation accuracy of the self-position of the moving body 2, and performs the estimation calculation of the self-position of the moving body 2. At this time, the lower the estimation accuracy of the self-position of the moving body 2, the wider the usable distance range of the detection data of the laser sensor 3. As the working distance range of the detection data of the laser sensor 3 becomes wider, the number of objects 7 that reflect the laser within the working distance range of the detected data tends to increase. As a result, the self-position of the moving body 2 can be estimated with high accuracy.

Further, in the present embodiment, when the self-position estimation accuracy of the moving body 2 is low, the usable distance range of the detection data of the laser sensor 3 is widened as compared with the case where the self-position estimation accuracy of the moving body 2 is high. , The self-position estimation calculation of the moving body 2 is performed. Therefore, when the estimation accuracy of the self-position of the moving body 2 is low, the working distance range of the detection data of the laser sensor 3 becomes wide, so that the number of objects 7 that reflect the laser within the working distance range of the detected data is surely increased. It becomes easy to increase.

Further, in the present embodiment, it is detected whether or not the estimation accuracy of the self-position of the moving body 2 is lowered by the estimation calculation unit 11, and when the estimation accuracy of the self-position of the moving body 2 is not lowered, the laser sensor is used. When the self-position estimation calculation of the moving body 2 is performed using the normal distance range P as the usage distance range of the detection data of 3 and the estimation accuracy of the self-position of the moving body 2 is lowered, the laser sensor Using the long distance range Q as the usage distance range of the detection data of 3, the self-position estimation calculation of the moving body 2 is performed. As described above, as the working distance range of the detection data of the laser sensor 3, only two, the normal distance range P and the long distance range Q, are used. Therefore, the arithmetic processing executed by the estimation arithmetic unit 11 can be simplified.

Further, in the present embodiment, by performing the estimation calculation using the normal distance range P, the self-position of the moving body 2 is estimated as the first position estimated value, and the AR marker 8 detected by the camera 4 is used. Based on this, the self-position of the moving body 2 is estimated as the second position estimate. Then, when the amount of deviation between the first position estimated value and the second position estimated value is equal to or greater than the threshold value, it is detected that the estimation accuracy of the self-position of the moving body 2 is lowered. By comparing the first position estimated value obtained based on the detection data of the laser sensor 3 with the second position estimated value obtained based on the AR marker 8 installed on the road surface, the moving body It is detected whether or not the estimation accuracy of the self-position of 2 is lowered. Therefore, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body 2 is lowered.

Further, in the present embodiment, when the deviation amount between the first position estimated value and the second position estimated value is continuously equal to or greater than the threshold value for a predetermined time, the estimation accuracy of the self-position of the moving body 2 is lowered. Is detected. Therefore, since the state in which the deviation amount between the first position estimated value and the second position estimated value momentarily exceeds the threshold value is excluded, it is more accurate whether or not the estimation accuracy of the self-position of the moving body 2 is lowered. Can be detected.

Further, in the present embodiment, the used distance range of the detection data of the laser sensor 3 is automatically changed according to the estimation accuracy of the self-position of the moving body 2, so that the estimation accuracy of the self-position of the moving body 2 is required. It is not necessary for the operator to search for and specify a suitable location. Therefore, the burden on the operator and the like can be reduced.

In the present embodiment, the AR marker 8 installed on the road surface is imaged by the camera 4, the self-position of the moving body 2 is estimated based on the image data of the AR marker 8, and the estimation result is estimated by the estimation calculation unit 11. It is used to detect whether or not the estimation accuracy of the self-position of the moving body 2 is lowered, but the form is not particularly limited.

For example, a barcode may be used as a mark installed on the road surface. In this case, the barcode is read by the barcode reader, the self-position of the moving body 2 is estimated based on the reading data of the barcode, and the estimation result is the estimation of the self-position of the moving body 2 by the estimation calculation unit 11. It is used to detect whether the accuracy has deteriorated.

FIG. 7 is a schematic configuration diagram showing a travel control device including the position estimation device according to the second embodiment of the present invention. In FIG. 7, the position estimation device 15 of the present embodiment includes a measuring instrument 20 for measuring the state quantity of the moving body 2 instead of the camera 4.

The measuring instrument 20 includes, for example, an encoder that measures the rotation angle of the wheels of the moving body 2 as the state quantity of the moving body 2, an inertial measurement unit that measures the angular velocity and acceleration of the moving body 2 as the state quantity of the moving body 2. Used. The measuring instrument 20 constitutes a state detection unit that detects the state of the moving body 2.

Further, the position estimation device 15 includes an estimation accuracy detection unit 12A instead of the estimation accuracy detection unit 12 described above. The estimation accuracy detection unit 12A detects the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 based on the state quantity of the moving body 2 measured by the measuring instrument 20. Specifically, the estimation accuracy detection unit 12A detects whether or not the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered based on the state quantity of the moving body 2.

FIG. 8 is a flowchart showing details of the detection processing procedure executed by the estimation accuracy detection unit 12A, and corresponds to FIG. In FIG. 8, the estimation accuracy detection unit 12A first acquires the measured value of the measuring instrument 20 (procedure S111A). Then, the estimation accuracy detection unit 12A estimates the self-position of the moving body 2 as the second position estimation value based on the measurement value of the measuring instrument 20 (procedure S112A). After that, the estimation accuracy detection unit 12A sequentially executes the above steps S113 to S119.

In such an embodiment, by comparing the first position estimated value obtained based on the detection data of the laser sensor 3 with the second position estimated value obtained based on the state of the moving body 2. , It is detected whether or not the estimation accuracy of the self-position of the moving body 2 is lowered. Therefore, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body 2 is lowered.

FIG. 9 is a schematic configuration diagram showing a travel control device including a position estimation device according to a third embodiment of the present invention. In FIG. 9, the position estimation device 15 of the present embodiment does not include the above-mentioned camera 4 and measuring instrument 20.

Further, the position estimation device 15 includes an estimation accuracy detection unit 12B instead of the estimation accuracy detection units 12 and 12A described above. The estimation accuracy detection unit 12B detects the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 based on the detection data of the laser sensor 3. Specifically, the estimation accuracy detection unit 12B detects whether or not the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered based on the detection data of the laser sensor 3.

FIG. 10 is a flowchart showing details of the detection processing procedure executed by the estimation accuracy detection unit 12B, and corresponds to FIG. In FIG. 10, the estimation accuracy detection unit 12B first acquires the detection data of the laser sensor 3 (procedure S121).

Subsequently, the estimation accuracy detection unit 12B calculates the variance value of the plurality of self-position candidate points S (see FIG. 4) of the moving body 2 (procedure S122). Specifically, the estimation accuracy detection unit 12B first calculates the average value of a plurality of self-position candidate points S, calculates the difference between each self-position candidate point S and the average value as a standard deviation, and calculates the standard deviation of the standard deviation. Calculate the root mean square as the variance value.

Subsequently, the estimation accuracy detection unit 12B determines whether or not the variance value of the plurality of self-position candidate points S is equal to or greater than a predetermined threshold value (procedure S123). When the estimation accuracy detection unit 12B determines that the variance value of the plurality of self-position candidate points S is equal to or greater than the threshold value, the state in which the dispersion value of the plurality of self-position candidate points S is equal to or greater than the threshold value is the specified number of times (specified cycle). ) Determine whether it is continuous (procedure S124). That is, the estimation accuracy detection unit 12B determines whether or not the variance values of the plurality of self-position candidate points S are continuously equal to or greater than the threshold value for a predetermined time.

When the estimation accuracy detection unit 12B determines that the state in which the variance values of the plurality of self-position candidate points S are equal to or greater than the threshold value is continuous for a predetermined number of times, the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 Is determined to be reduced (procedure S125).

When the estimation accuracy detection unit 12B determines in the procedure S123 that the variance values of the plurality of self-position candidate points S are not equal to or greater than the threshold value, or in the procedure S124, the dispersion values of the plurality of self-position candidate points S are equal to or greater than the threshold value. When it is determined that the number of consecutive times is not continuous, it is determined that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 has not deteriorated (procedure S126).

Subsequently, the estimation accuracy detection unit 12B outputs the result determined in the procedure S125 or the procedure S126 to the estimation calculation unit 11 as an estimation accuracy detection signal (procedure S127). After that, the estimation accuracy detection unit 12B executes the above procedure S121 again.

When detecting the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 using the variance values of the plurality of self-position candidate points S in this way, as shown in FIG. 11, for example, the self-position candidate The laser visible from the point S is projected onto the map, and the degree of matching between the self-position candidate point S and the map is measured. The self-position candidate point S1 shown in FIG. 11A has a high degree of matching with the map. The self-position candidate point S2 shown in FIG. 11B has a low degree of matching with the map.

As shown in FIG. 12, when there are many self-position candidate points S1 having a high degree of matching with the map, the variance value of the plurality of self-position candidate points S becomes small. Therefore, it is detected that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 has not deteriorated. The self-position candidate point S1 in FIG. 12 is a self-position candidate point S having a high degree of matching with the map. The self-position candidate point S2 in FIG. 12 is a self-position candidate point S having a low degree of matching with the map.

As shown in FIG. 13A, when there are many self-position candidate points S2 having a low degree of matching with the map, the variance value of the plurality of self-position candidate points S becomes large. Therefore, it is detected that the estimation accuracy of the self-position of the moving body 2 by the estimation calculation unit 11 is lowered. The self-position candidate point S1 in FIG. 13 is also a self-position candidate point S having a high degree of matching with the map. The self-position candidate point S2 in FIG. 13 is also a self-position candidate point S having a low degree of matching with the map.

When there are many self-position candidate points S2 having a low degree of matching with the map, the range for creating the self-position candidate points S may be expanded as shown in FIG. 13 (b). Even if the range for creating the self-position candidate point S is expanded, the accuracy of estimating the self-position of the moving body 2 by the estimation calculation unit 11 is lowered when there are many self-position candidate points S2 having a low degree of matching with the map. May be detected.

In the present embodiment as described above, the dispersion value of the self-position candidate point S of the moving body 2 is calculated based on the detection data of the laser sensor 3, and when the dispersion value is equal to or more than the threshold value, the moving body 2 It is detected that the estimation accuracy of the self-position is low. By the calculation process based on the detection data of the laser sensor 3 in this way, it is detected whether or not the estimation accuracy of the self-position of the moving body 2 is lowered. Therefore, since the camera 4 and the measuring instrument 20 are not required, it is possible to detect whether or not the estimation accuracy of the self-position of the moving body 2 is lowered with a simple configuration.

Further, in the present embodiment, when the variance value of the self-position candidate point S is continuously equal to or higher than the threshold value for a predetermined time, it is detected that the estimation accuracy of the self-position of the moving body 2 is lowered. Therefore, since the state in which the dispersion value momentarily exceeds the threshold value is excluded, it is possible to accurately detect whether or not the estimation accuracy of the self-position of the moving body 2 is lowered.

The present invention is not limited to the above embodiment. For example, in the above embodiment, when the amount of deviation between the first position estimated value and the second position estimated value or the dispersion value of the plurality of self-position candidate points S of the moving body 2 is continuously equal to or greater than the threshold value for a specified time. It is detected that the estimation accuracy of the self-position of the moving body 2 is lowered, but the form is not particularly limited. When the amount of deviation between the first position estimated value and the second position estimated value or the dispersion value of the plurality of self-position candidate points S of the moving body 2 is continuously equal to or greater than the threshold value for a predetermined distance, the self-position of the moving body 2 It may be detected that the estimation accuracy of is lowered. Further, when the deviation amount between the first position estimated value and the second position estimated value or the dispersion value of the plurality of self-position candidate points S of the moving body 2 becomes equal to or more than the threshold value, the estimation accuracy of the self-position of the moving body 2 May be detected as decreasing.

Further, in the above embodiment, it is detected whether or not the estimation accuracy of the self-position of the moving body 2 is lowered, and when the estimation accuracy of the self-position of the moving body 2 is not lowered, the detection data of the laser sensor 3 is detected. When the normal distance range P is used as the used distance range and the estimation accuracy of the self-position of the moving body 2 is low, the long distance range Q is used as the used distance range of the detection data of the laser sensor 3. However, it is not particularly limited to that form. For example, the self-position estimation accuracy of the moving body 2 is detected in three or more ranges such as "high", "medium", and "low", and the lower the self-position estimation accuracy of the moving body 2 is, the laser sensor The range of use distance of the detection data of 3 may be widened in 3 or more steps.

Further, the above-described embodiment is a travel control device for traveling the moving body 2 to a destination along a traveling route, but the present invention is not particularly limited to this, and for example, the moving body 2 is traveled along an object. This is applicable to any device that needs to estimate the self-position of the moving body 2, such as the case.

Further, in the above embodiment, the self-position of the moving body 2 is estimated by using the SLAM method using a laser as the self-position estimation technique, but the present invention is not particularly limited to the SLAM method and uses a laser. Any method of estimating the self-position of the moving body 2 based on the detection data of the sensor that detects the distance can be applied.

2 ... moving body, 3 ... laser sensor (distance detection unit), 4 ... camera (mark detection unit), 7 ... object, 8 ... AR marker (mark), 11 ... estimation calculation unit, 12, 12A, 12B ... estimation accuracy Detection unit, 15 ... position estimation device, 20 ... measuring instrument (state detection unit), L ... laser, P ... normal distance range (first distance range), Q ... long distance range (second distance range), S ... self Position candidate point.

Claims (8)

In a position estimation device that estimates the self-position of a moving object,
A distance detection unit that detects the distance to an object around the moving body by irradiating the surroundings of the moving body with a laser and receiving the reflected light of the laser.
An estimation calculation unit that performs an estimation calculation of the self-position of the moving body based on the detection data of the distance detection unit, and
It is provided with an estimation accuracy detection unit that detects the estimation accuracy of the self-position of the moving body by the estimation calculation unit.
The estimation calculation unit changes the used distance range of the detection data of the distance detection unit according to the estimation accuracy of the self-position of the moving body detected by the estimation accuracy detection unit, and determines the self-position of the moving body. A position estimation device that performs estimation calculations. When the estimation accuracy of the self-position of the moving body is low, the estimation calculation unit widens the usable distance range of the detection data of the distance detection unit as compared with the case where the estimation accuracy of the self-position of the moving body is high. The position estimation device according to claim 1, wherein the self-position estimation calculation of the moving body is performed. The estimation accuracy detection unit detects whether or not the estimation accuracy of the self-position of the moving body by the estimation calculation unit is lowered.
When the estimation accuracy of the self-position of the moving body is not lowered, the estimation calculation unit uses the first distance range as the used distance range of the detection data of the distance detecting unit, and the self-position of the moving body When the estimation accuracy of the self-position of the moving body is low, a second distance range wider than the first distance range is used as the used distance range of the detection data of the distance detection unit. The position estimation device according to claim 2, wherein the self-position estimation calculation of the moving body is performed. A mark detection unit for detecting a mark installed on the road surface on which the moving body moves is further provided.
The estimation calculation unit estimates the self-position of the moving body as a first position estimation value by performing an estimation calculation of the self-position of the moving body using the first distance range.
The estimation accuracy detection unit estimates the self-position of the moving body as a second position estimation value based on the mark detected by the mark detection unit, and the first position estimation value and the second position estimation value. The position estimation device according to claim 3, wherein it is detected that the estimation accuracy of the self-position of the moving body is lowered when the deviation amount from the above is equal to or more than the threshold value. A state detection unit for detecting the state of the moving body is further provided.
The estimation calculation unit estimates the self-position of the moving body as a first position estimation value by performing an estimation calculation of the self-position of the moving body using the first distance range.
The estimation accuracy detection unit estimates the self-position of the moving body as a second position estimated value based on the state of the moving body detected by the state detecting unit, and the first position estimated value and the second position estimated value. The position estimation device according to claim 3, wherein when the amount of deviation from the position estimation value is equal to or greater than a threshold value, it is detected that the estimation accuracy of the self-position of the moving body is lowered. The estimation accuracy detection unit estimates the self-position of the moving body when the amount of deviation between the first position estimated value and the second position estimated value is continuously equal to or greater than the threshold value for a specified time or a specified distance. The position estimation device according to claim 4 or 5, which detects that the accuracy is reduced. The estimation accuracy detection unit calculates the dispersion value of a plurality of self-position candidate points of the moving body based on the detection data of the distance detection unit, and when the dispersion value is equal to or more than the threshold value, the moving body The position estimation device according to claim 3, wherein it detects that the accuracy of self-position estimation is low. The seventh aspect of claim 7, wherein the estimation accuracy detection unit detects that the estimation accuracy of the self-position of the moving body is lowered when the dispersion value is continuously equal to or higher than the threshold value for a specified time or a specified distance. Position estimation device.

Images