JP2023093268A - Position detector and method of detecting position - Google Patents

Abstract

To provide a position detector allowed to accurately detect a position of a moving body even in an environment no positional detection is allowed under ordinary circumstances.SOLUTION: A position detector comprises a mirror-distance data extracting section that extracts mirror-distance data by a distance sensor using a mirror and a position detecting section that detects a position of a moving body by indirectly detecting a structure using the mirror-distance data.SELECTED DRAWING: Figure 1

Description

The present invention relates to a position detection device and a position detection method.

In recent years, with the increase in physical distribution in factories, technology has been developed to detect the position and orientation of forklifts by installing distance sensors such as laser scanners and applying SLAM (Simultaneous Localization And Mapping) technology. there is

AGVs (Automated Guided Vehicles) that utilize this technology can improve transport efficiency by moving autonomously. It is a system that detects the position and orientation by shape matching the measurement data of the mounted distance sensor. For this reason, accuracy may decrease depending on the shape of the environment.

In addition, due to the presence of items such as packages that were not present when the map was created, the matching rate with the map decreases, leading to a decrease in accuracy. A sensor may be required.

There is Patent Document 1 as a prior art document in this technical field. Japanese Patent Laid-Open No. 2004-100002 discloses a technique of extending the optical path of a laser by attaching a mirror inside a base station in order to improve the positioning accuracy with respect to the base station.

Japanese Patent Publication No. 2018-526748

However, in Patent Document 1, since a mirror is attached to the back of the base station, the laser light hits the mirror almost perpendicularly when measured at a position some distance away from the base station. Therefore, it is impossible to measure the inside surface of the base station to be measured.

SUMMARY OF THE INVENTION An object of the present invention is to detect the position of a moving body with high precision in a position detection device even in an environment where the position cannot normally be detected.

A position detection device according to one aspect of the present invention estimates its own position by comparing distance data measured by a distance sensor with map data of a work site, and autonomously moves along a movement route within the work site. A position detection device for a moving object, wherein at least a structure and a mirror are arranged at the work site, and the position detection device uses the mirror to extract mirror distance data by means of the distance sensor. It is characterized by having a distance data extraction unit and a position detection unit that detects the position of the moving object by indirectly detecting the structure using the mirror distance data.

According to one aspect of the present invention, the position detection device can detect the position of a moving object with high accuracy even in an environment where position detection is normally impossible.

It is a figure which shows the system configuration example for demonstrating this invention. FIG. 4 is a diagram showing a format example of map data; It is a figure which shows the example of the site where a mobile body operate|moves. It is a figure which shows the detection example of the position using the data of a mirror. It is a figure which shows the flowchart of the position detection using a mirror. It is a figure which shows the approach example to the installation of a moving body. It is a figure which shows the example of positioning using the mirror installed in the equipment. It is a figure which shows the flowchart of the positioning process in an installation. It is a figure which shows the other system configuration example for demonstrating this invention. FIG. 5 is a diagram showing processing of a mirror distance data generation unit and a mirror position/orientation calculation unit; It is a figure which shows the other flowchart of the position detection using a mirror.

The present invention relates to a position detection device for detecting the position and posture of moving bodies such as forklifts and automatic guided vehicles AGVs operating in factories. An embodiment will be described below with reference to the drawings.

In the present embodiment, a description will be given by taking as an example an operation in which a moving body (AGV) 10 conveys parts in a factory. FIG. 1 is a system configuration diagram that constitutes the entire embodiment.

The moving body 10 includes a distance sensor 105 for detecting the position, a position detection device 104 for detecting the position of the moving body 10 from distance data that is measurement data of the distance sensor 105, and a driving wheel 121 for controlling the driving wheel 121. A vehicle control device 120 is mounted. In addition, there is a transfer device 102 for exchanging packages with facilities provided on site, and the transfer device 102 is used to transfer the package 101 .

The position detection device 104 is a device for detecting the position and orientation of the moving object 10, and includes a distance sensor control unit 106, a position and orientation detection unit 107, a movement path calculation unit 113, a mirror distance data extraction unit 110, a relative position and orientation. It has a calculator 111 , a control command generator 112 , and a predicted position/orientation generator 108 . These are primarily processed by processor 119 .

The position detection device 104 also has a memory 118 . The memory 118 stores a map 114a, a moving route 114b, a mirror map type 117a, a mirror position/attitude and width 117b, a mirror map 117c, and a positioning target position/attitude 117d as data.

A distance sensor control unit 106 is used to control the distance sensor 105 . A distance sensor 105 is a sensor capable of measuring the distance to an object in the surrounding environment and the measurement direction. In this embodiment, a two-dimensional laser scanner is used as a typical distance sensor 105 for explanation. A two-dimensional laser scanner emits laser pulses in the horizontal direction and measures the time it takes for the light to hit the object and diffusely reflect the laser light to return, thereby calculating the distance to the object. It is.

A motor (not shown) provided inside the distance sensor 105 rotates the irradiation direction of the laser pulse, and by obtaining the irradiation direction and the distance, it is possible to obtain the shape data of the surroundings.

The measurement range of the laser scanner is, for example, an irradiation range of 270 degrees, an angular resolution (pulse irradiation interval) of 0.25 degrees, and a scanner capable of acquiring distance data of 1081 points at a cycle of 25 msec. The present invention is applicable not only to such a scanner, but also to any sensor capable of acquiring a plurality of pairs of distance data and irradiation directions even if the irradiation range and angular resolution are different.

In addition to 2D laser scanners, 3D laser scanners and stereo cameras that can measure the distance to surrounding structures in 3D can also acquire multiple distances to surrounding structures in different angular directions. It is possible to apply the present invention as long as it is possible.

The position and orientation detection unit 107 detects the position and orientation of the moving body 10 by comparing the distance data and the map 114 .

A mirror distance data extraction unit 110 extracts a portion assumed to be measuring a mirror in the laser measurement data. In this extraction processing, the predicted position/orientation generation unit 108 is utilized, and the details of these processing will be described later.

The movement route calculation unit 113 calculates a movement route for the moving body 10 to move. A relative position/posture calculation unit 111 detects the relative position/posture of the moving body 10 . The control command generation unit 112 generates commands for controlling the moving body 10 . A predicted position and orientation generation unit 108 generates a predicted position and orientation of the moving body 10 .

In this embodiment, the processor 119 is an arithmetic unit for executing a program, such as a CPU (Central Processing Unit), but any device capable of executing a program may be used, such as an FPGA (Field Programmable Gate). Array), CPLD (Complex Programmable Logic Device), ASIC (application specific integrated circuit), or the like.

The vehicle control device 120 is a control device for guiding the moving body 10 based on the relative position and orientation calculated by the relative position and orientation calculation section 111 .

FIG. 2 shows the information written on the map 114a and the information written on the mirror map 117c.
The map 114a describes the positional information of the surrounding structures. For example, a space called an occupancy grid map is divided into grids for the positions of structures. expressed in the form However, any expression method may be used as long as it is possible to determine whether it matches the laser data, and a Normal Distribution Model or the like that approximates the point group data in the lattice with a Gaussian distribution may be used.

The movement route 114b describes information on the route along which the moving body 10 moves, and is composed of a target position and a posture. The moving body 10 selects a target position/orientation from the movement path and moves toward the target position.

The mirror map number is the origin information of the mirror map that utilizes the mirrors existing on site. This origin information is linked to the target point of the moving route. When moving toward the target point to which the origin information of the mirror map 117c is linked, the moving body 10 develops the mirror map 117c on the memory 118 and executes position and orientation detection processing while referring to it. . Details of these processes will be described later.

Each mirror map 117c has a type, and is distinguished between normal driving and positioning. The map for normal driving is a mirror map provided to give characteristics in the environment where SLAM is not suitable. The positioning map is for highly accurate positioning when docking with facilities. A method of utilizing this map will be described in a second embodiment.

The position and orientation of the mirror represent the reference position of the mirror map. In the case of a single mirror, the direction perpendicular to the surface of the mirror is taken as the x-axis, the direction along the direction of movement is taken as the y-axis, and the vertical direction is taken as the z-axis.

The mirror map 117c is map data obtained by mirror-reflecting the map data of the map 114a, and can be obtained as a mirror image of the necessary portion of the data described on the map 114a.

A mirror image in the two-dimensional case can be obtained by folding the map 114a against the plane of the mirror. Details will be described with reference to FIG.

FIG. 3 shows the site where the mobile unit 10 operates and the map data of the site map 114a.
The map data of the map 114a is like a cross-sectional view obtained by slicing a structure arranged on site at a constant height. This map data can be created by SLAM technology or the like, but if precise CAD data or the like exists, this data can also be used as the map 114a for position detection.

Here, in FIG. 3, 305 indicates the origin of the map, 301 indicates the mirror, 302 indicates the range of the mirror map 117c, and 304 indicates the transfer device (equipment side).

The map data also describes the moving route 114b of the moving body 10. FIG. In this embodiment, the moving route is divided into sections. A target position/orientation (wx, wy, wθ) is set for each section, a curvature k representing a path, and presence/absence MN of the mirror 301 are set.

The moving body 10 measures the surrounding environment with the distance sensor 105, identifies its own position by comparing it with the map 114a, and adjusts the drive wheels 121 so as to run on the target position/orientation of the current running section or on the moving route. The motion is performed while controlling the rotation of the

The presence/absence MN of the mirror 301 shown in FIG. 2 takes a value of 0 when there is no mirror in the traveling section, and a value of 1 to L, which is the number of the mirror, when the mirror 301 exists. When MN is 1 to L, the position detection device 104 refers to the set mirror map 117c and uses the mirror map 117c together to detect the position and orientation.

FIG. 4 shows an example of position and orientation detection using a mirror map. A mirror map 117c is generated by folding back the map data of the map 114a with a line along the y-axis of the origin 401 of the mirror 201 as a line of symmetry.

In the general case including three dimensions, it is a mirror map with the x-axis of the mirror map origin as a normal vector, and more specifically, it can be obtained using the Householder transformation.

The distance data for shape matching processing with the mirror map 117c is based on the predicted position and orientation 10n considering the movement amount 403 of the moving body 10, and the data 404 that would have measured the mirror 201 is used as the positional relationship with the mirror 201. and the dimensions of the mirror 201 to extract. At this time, the movement amount 403 is calculated from the command value when controlling the moving body 10 . Therefore, the actual amount of movement is not measured, and since some error is included, the extracted distance data also includes the mirror 201 that is not actually measured.

However, by widening the width 402 of the mirror 201 to some extent, even if the wrong data is selected, the matching process with the mirror map 117c can be performed with almost no influence. In addition, a Kalman filter or the like may be used to estimate the velocity and acceleration to generate an estimated value of the position and orientation after several seconds.

At this time, the data measured through the mirror includes data 405 that cannot be measured without the mirror 201 . Without the mirror 201, the only data that can be measured from the position of the moving body 10 would be information about the wall along the traveling direction of the passageway. For this reason, although the distance from the wall is known, information for positioning with respect to the traveling direction is insufficient. By using the data 405 measured through the mirror, it is possible to obtain information such as how far it has progressed in the traveling direction.

FIG. 5 is a flowchart of position and orientation detection using a mirror map. A detailed procedure of the processing will be described with reference to FIG.
First, the position/orientation detection process is started from the acquisition of distance data (step 501). It is determined whether or not the mirror map 117c is set for the section of the current route (step 502). If the mirror map 117c is not set, shape matching processing with the map 114a is performed using the acquired distance data (step 508).

This shape matching process can be performed using a technique such as scan matching such as an ICP (Iterative Closest Point Search) algorithm or an NDT (Normal Distribution Transform) algorithm. In addition, the present invention is not limited to this, and any technology that can compare the shapes of each other and specify the matching position may be used, and an AI technology such as deep learning may be used.

If there is setting information for the mirror map 117c in the current section, the mirror map 117c set for the route is developed on the memory 118 (step 503).

Next, from the position and orientation calculated from the distance data one sample before, the position and orientation after movement are estimated using values such as control commands. Using this estimated position and orientation and information on the position, orientation and width of the mirror, distance data that is highly likely to be reflected by the mirror is extracted (step 504).

Next, the distance data extracted in step 504 and the mirror map 117c are collated (step 505). Processing for calculating the position and orientation can be performed in the same manner as in step 508 .

Next, using the distance data in which the mirror is not measured, comparison processing with the map 114a is performed (step 506).

Next, fusion processing is performed on the position and orientation calculated in step 505 and the position and orientation calculated in step 506, respectively. For the fusion process, the simplest method is to simply take the average value, but if the covariance matrix can be calculated as a result of the matching process, take the weighted average value with this covariance matrix as the weight. By doing so, accuracy can be improved. The above is the position/orientation detection processing using the mirror.

In a second embodiment, an example will be described in which mirrors are arranged according to the environment and positioned with high accuracy. FIG. 6 shows how the load 101 carried by the moving body 10 is transferred to the transfer device 304 . A matching mirror is installed inside the transfer device 304 . The load 101 transferred by the transfer device 304 is placed on the workbench 601 .

FIG. 7 shows a state in which the moving body 10 is actually measuring the environment near the transfer device 304 .
Temporarily placed cargo 701 and workers 702 are present around the transfer device 304, and the surrounding structures cannot be measured with sufficient accuracy. Therefore, the detection accuracy of the position and orientation is low when the transfer device 304 reaches the approach position.

A matching mirror 704 a is installed in the transfer device 304 . In this embodiment, the matching mirrors 704a are installed so that the angle between them is 90 degrees. Markers 703 are installed on the matching mirror 704a, and the markers 703 can be arranged regularly in the space by setting the angle in this way.

Thus, by setting an angle between the alignment mirrors 704a, it is possible to increase the distance from the marker 703 in a pseudo manner. Also, the laser is not irradiated perpendicularly to the matching mirror 704a. For this reason, it is possible to measure an object sufficiently serving as a landmark, and it is possible to perform positioning processing with high accuracy.

At this time, the position and orientation information output from the relative position/orientation calculation unit 111 is output as the position and orientation in the coordinate system 705 with the relative target position/orientation as the origin. This position/orientation information is deviation information from a target position when positioning on the transfer device 304 . The control command generation unit 112 generates a control command so as to set this deviation to 0, and inputs the control command to the vehicle control device 120, whereby positioning can be performed. By performing control from the position detection device 104 in this way, the moving body 10 can be positioned with high accuracy regardless of changes in the surrounding environment.

Although two mirrors 704a are used in this embodiment, a plurality of mirrors may be used. Also, although the present embodiment uses a two-dimensional laser scanner for explanation, a three-dimensional laser scanner can also be used. In that case, the matching mirror 704a can be arranged three-dimensionally, and for example, the mirror may be attached to a rectangular surface.

Also, the mirrors are arranged so that the angle between any two mirrors is either 45 degrees, 60 degrees, 90 degrees, or 180 degrees. By arranging the folding mirrors 704a so as to take the angles represented by the Dynkin diagrams, the folds of the space surrounded by the mirrors can be developed in the space without regular overlap. When the mirrors 704a are arranged in this way, the mirror images 703m of the markers 703 are arranged regularly, so that the mirror map 117c can be easily created.

In this case, the matching process between the distance data and the mirror map 117c may use the scan matching method as described above, or may use the triangulation method based on the information of the irradiation angle of the laser.

For example, the positioning process can be performed by extracting distance data for measuring the marker 703 and calculating its direction vector. As a method for specifically calculating the position and orientation of the moving body 10, for example, a technique such as the Perspective-n-Point method used in the field of computer vision can be used. It can be calculated with high precision.

FIG. 8 shows a flowchart of positioning processing for this map data. This flowchart shows the flow from acquisition of distance data to detection of position and orientation.

First, distance data is obtained (step 801). Next, it is determined whether or not setting information of the mirror map 117c exists in the section of the current route (step 802). If not, the position and orientation are detected by collating the distance data with the map 114a (step 807).

When the information of the mirror map 117c is linked to the route, the information of the mirror map 117c set for the route is developed in the memory 118, and the mirror map 117c set for the route section is read out (step 803). . Next, the distance data for the range in which the mirror is measured is extracted (step 804).

Next, collation processing is performed using the distance data obtained by measuring the mirror, and the relative position and orientation with respect to the transfer device 304 are calculated (step 805). Next, the relative coordinates with reference to the transfer target position are output (step 806).

In this way, in the second embodiment, the mirror 704a can be arranged according to the environment and positioned with high accuracy.

FIG. 9 shows another aspect of the position/orientation detection device using a mirror. The configuration is basically the same as that of the first embodiment, but differs in that a mirror distance data generation unit 911 and a mirror image position/orientation calculation unit 912 are provided. The description of the configuration similar to that of the first embodiment is omitted. A mirror distance data generator 911 generates mirror distance data. A mirror image position/posture calculation unit 912 calculates a mirror image position/posture.

FIG. 10 shows the processing of the mirror distance data generation unit 911 and the mirror position/orientation calculation unit 912 . In the first embodiment, the map data is folded back by the mirror to detect the position and orientation. Position detection processing is performed using distance data 1002 obtained by folding back the distance data 1001 of the mirror 201 .

By performing processing in this way, it is possible to detect the position without folding the map data. The position/orientation folded back by the mirror 201 and the measurement data of the laser scanner are data consistent with the original map data. For this reason, it is possible to collate with the map data of the held normal map 114a. Since map data may consume a large amount of memory in some cases, this is an advantageous method from the viewpoint of memory efficiency.

Details of the processing of the third embodiment will be described with reference to FIG. First, distance data is obtained (step 1101). Next, it is determined whether or not there is partial map setting information in the section of the current route (step 1102). If not, the position and orientation are detected by collating the distance data with the map 114a (step 1110).

If there is partial map setting information for the section of the current route, the distance data for the range in which the mirror is being measured is extracted (step 1103). Next, a mirror image of the position and orientation and the extracted distance data is calculated (step 1104).

Next, matching processing is performed between the mirror image distance data calculated in step 1104 and the map data of the map 114a (step 1105). Next, a mirror image of the position and orientation obtained in step 1105 is calculated (step 1107).

Next, the distance data in which the mirror is not measured is matched with the map data of the map 114a (step 1108).

Next, the position and orientation calculated from the distance data measured through the mirror obtained in step 1107 and the position and orientation obtained from the distance data without measuring the mirror are merged (step 1109). As this fusion process, a weighted average value is obtained by using the covariance matrix of the degree of matching calculated when performing the matching process as a weight.

As described above, the position detection device of the above-described embodiment is a position detection device of a moving body that autonomously moves while estimating its own position by collating the distance data measured by the distance sensor with the map information of the moving environment. a mirror data extraction unit that uses a mirror to detect structures that cannot be directly measured by a distance sensor; and a position detection unit for performing

According to the above embodiment, the position and orientation of a mobile object can be accurately detected even in an environment where the position cannot normally be detected, such as when there are few features in the surrounding structures or when there are many objects not shown on the map. can be detected.

10 Moving body 101 Cargo 105 Distance sensor 102 Transfer device 104 Position/attitude detection device 119 Processor 106 Distance sensor control unit 107 Position/attitude detection unit 113 Movement route calculation unit 110 Mirror distance data extraction unit 111 Relative position/attitude calculation unit 112 Control command generation Unit 118 Memory 114a Map 117c Mirror map 120 Vehicle control device 121 Drive wheel 305 Map origin 301 Mirror 302 Mirror map range 304 Transfer device (facility side)
402 mirror width 703 positioning marker (mirror map)
703m Mirror image of positioning marker (mirror map)

Claims (14)

A position detection device for a moving body that autonomously moves along a movement route in the work site while estimating its own position by comparing distance data measured by a distance sensor with map data of the work site,
At least a structure and a mirror are arranged at the work site,
The position detection device is
a mirror distance data extraction unit for extracting mirror distance data by the distance sensor using the mirror;
a position detection unit that detects the position of the moving body by indirectly detecting the structure using the mirror distance data;
A position detection device comprising: The position detection unit is
2. The position detection device according to claim 1, wherein the position of the moving body is detected by collating the mirror distance data with pre-stored mirror map data. The position detection unit is
detecting the normal position of the moving body by comparing the distance data and the map data;
detecting a mirror image position of the moving object by comparing the mirror distance data and the mirror map data;
3. The position detection device according to claim 2, wherein the position of the moving body is detected by fusing the normal position and the mirror image position. The structure is
Having a transfer device on the equipment side arranged at the work site,
The mirror is installed in the transfer device,
The position detection unit is
calculating a relative position and orientation between the moving body and the transfer device by comparing the mirror distance data and the mirror map data;
3. The position detection according to claim 2, wherein the position of the moving object is detected by outputting relative coordinates with reference to the transfer target position of the transfer device based on the relative position and orientation. Device. 5. The position detecting device according to claim 4, wherein two or more mirrors are used as said mirrors. 6. The position detecting device according to claim 5, wherein the angle between said matching mirrors is represented by a Dynkin diagram. The position detection device is
a mirror distance data generation unit that generates folded mirror distance data generated by folding the mirror distance data extracted by the mirror data extraction unit with the mirror;
fusion processing of the normal position of the moving body detected by matching the distance data and the map data and the mirror image position of the moving body detected by matching the folded mirror distance data and the map data; a mirror image position detector for
2. The position detecting device according to claim 1, further comprising: The distance sensor is
is a two-dimensional laser scanner,
The two-dimensional laser scanner is
The distance is calculated by irradiating a laser pulse horizontally and measuring the time it takes for the diffusely reflected laser light to return.
The mirror data extraction unit
2. The position detection device according to claim 1, wherein the current position is predicted and data on intersection of the position of the mirror and the optical path of the laser is extracted. 9. The position detecting device according to claim 8, wherein a control command is used for predicting the current position. The map data includes
2. The position detection device according to claim 1, wherein position information of said structure and information of said moving route along which said moving body moves are described. The mirror map data is
3. The position detection device according to claim 2, wherein the map data is mirror image data of a part of the map data obtained by mirror-reflecting the map data, and is obtained by folding the map data with respect to the mirror. . A position detection device according to claim 1;
a control device that controls the moving body based on the position of the moving body detected by the position detection device;
A moving body characterized by having 13. The position detection system for the mobile body according to claim 12, which autonomously moves along the movement route within the work site. A position detection method for a moving object that autonomously moves along a movement route in a work site while estimating its own position by comparing distance data measured by a distance sensor with map data of the work site,
At least a structure and a mirror are arranged at the work site,
The position detection method includes:
a mirror distance data extraction step of extracting mirror distance data by the distance sensor using the mirror;
a position detection step of detecting the position of the moving body by indirectly detecting the structure using the mirror distance data;
A position detection method characterized by comprising:

Images