JP6923455B2 - Position detection system - Google Patents

Description

The present invention relates to a position detection system.

In recent years, a technique of generating an environment map from measurement data such as a distance sensor attached to a moving body and detecting the position and orientation of the moving body on the map has been put into practical use. The position and orientation of a moving object on a map are generally detected by collating the map data with the measurement data.

For example, Patent Document 1 discloses a position detection system that estimates its own position and posture by collating the measurement data measured by a distance sensor that measures the surrounding situation with the map data.

Japanese Unexamined Patent Publication No. 2015-094669

However, in the position detection system of Patent Document 1, it is difficult to accurately estimate the self-position when the measurement data measured by the distance sensor is applied to a plurality of points on the map.
An object of the present invention is to accurately estimate a self-position in a position detection system.

The position detection system of one aspect of the present invention is a position detection system for a moving body that estimates its own position in an operating region in which the moving body moves, and the moving body is a first sensor that measures the surrounding situation. A second sensor that sets a plurality of data acquisition areas that can be distinguished from each other in the operation area, and a position estimation unit that estimates the self-position, and the position estimation unit is the second. The plurality of data acquisition areas set by the sensors of the above and the map data in the operation area are linked in advance.
The map data is collated based on the measurement data measured by the first sensor and the data acquisition area set by the second sensor, and the self-position is narrowed down to estimate the self-position. It is characterized by that.

According to one aspect of the present invention, the self-position can be accurately estimated in the position detection system.

It is a figure which shows the structure of the position detection system. It is a figure which shows the usage mode of a position detection system. It is a figure which shows the other use mode of a position detection system. It is a flowchart for demonstrating operation of a position detection system. It is a figure which shows an example of the case where the self-position candidate search area can be narrowed down by an additional sensor. It is a flowchart for demonstrating operation of a position detection system.

Generally, the position and posture of a moving object on a map are detected by collating the measurement data measured by a distance sensor that measures the surrounding conditions with the map data.

However, it is difficult to accurately estimate the self-position when the measurement data measured by the distance sensor is applied to a plurality of points on the map only by collating the measurement data measured by the distance sensor with the map data. Such an event is particularly likely to occur in the estimation of the initial position.
Specifically, in the estimation of the initial position, the correct map is selected by using only the measurement data that can be acquired by the distance sensor among the multiple maps possessed by the position detection system, and then the self-position on the map is determined. Need to estimate.

However, there is a possibility that matching positions exist on a plurality of maps based only on the distance data from the surrounding environment. Alternatively, even a single map may apply to multiple locations.
When the above-mentioned event occurs in the estimation of the initial position, an external input such as a manual input of the initial position or a device movement operation by the operator is required. Therefore, the position detection system may not operate as expected. In addition, automation of the transport system using the position detection system may not operate as expected.

Further, when there are many candidates for self-position estimation, many calculations are required to confirm the candidates, and the time required to complete the self-position estimation becomes long.

In the embodiment, in the position detection system using the distance sensor, when there are a plurality of self-position estimation candidates, it is possible to estimate an accurate self-position. Specifically, in the estimation of the self-position, in addition to the surrounding conditions that can be acquired from the distance sensor, the data that can be acquired by the additional sensor is linked to the map and obtained by the additional sensor when the self-position is estimated. The self-position candidates are narrowed down by referring to the data.

In the embodiment, additional sensors are added according to the usage environment, and additional data is registered in the map when the position detection system is newly introduced or when the additional sensors are added to the position detection system. This makes it possible to estimate the self-position even when it is difficult to estimate the self-position only with the distance sensor, and the accuracy of estimating the self-position is improved.

In addition to the estimation based on the distance to the surrounding environment, by narrowing down the candidates for the self-position by the signal received by the additional sensor, the number of comparison processing operations required for the self-position estimation is reduced, and the self-position can be estimated. The time required is shortened.
Hereinafter, examples will be described with reference to the drawings.

The configuration of the position detection system will be described with reference to FIG.
As shown in FIG. 1, the position detection system includes a position / orientation estimation device 101, a distance sensor (first sensor) 110, and an additional sensor (second sensor) 111. The position / orientation estimation device 101, the distance sensor 110, and the additional sensor 111 are mounted on the moving body 201 (see FIGS. 2A and 2B).

The position / orientation estimation device 101 includes an arithmetic unit (processor) 102 such as a central processing unit (CPU: Central Processing Unit), a memory 103, a communication device 104, and a storage device 105 such as a hard disk.

The storage device 105 stores a position / orientation estimation program 106, a communication program 107, a sensor control program 108, and a map (map data) 109.

The arithmetic unit 102 comprehensively controls each component by executing the program stored in the storage device 105, and performs various arithmetic processes.

The distance sensor 110 irradiates a laser beam with a rotating prism or the like in various directions to measure the surrounding condition (that is, the distance to a surrounding object). Since the irradiation angle of the distance data is known, the two-dimensional coordinates of the measured data can be restored, and the geometric shape of the measurable object around the distance sensor 110 is measured and acquired. can do.

The sensor control program 108 controls the laser irradiation of the distance sensor 110 and processes information such as the distance data and the angle obtained by the distance sensor 110.

The map data 109 is map data in which the shapes of objects in the surrounding environment are described in advance.
The position / orientation estimation program (position / orientation estimation unit) 106 moves by collating the distance data, which is the measurement data of the distance sensor 110, with the map data 109, which describes the shape of an object in the surrounding environment created in advance. The position and posture of the body 201 (see FIGS. 2A and 2B) are estimated.

The distance sensor 110 mounted on the moving body 201 measures the shape of an object in the surrounding environment. All the shapes of the objects are described in advance in the map data 109. Therefore, the position / orientation estimation program 106 matches the map data 109 while changing the distance data in various positions and directions, and obtains the most matching position and orientation. As a result, the position and orientation of the moving body 201 on the map 109 are detected.
Here, the "position and posture" are the position (for example, X and Y coordinates) of the moving body 201 on the map 109 and the direction (angle) of the moving body 201.

The additional sensor 111 is, for example, a receiver of information that can be acquired in the area where the position detection system is used.
Based on the information received by the additional sensor 111 and the information that can be received by the additional sensor 111 linked to the map 109, the position / orientation estimation program 106 narrows down the information. Then, the self-position is estimated from the surrounding conditions acquired by the distance sensor 110. Here, there may be a plurality of maps 109.

Next, the usage mode of the position detection system will be described with reference to FIGS. 2A and 2B.
As an example, the information acquired by the additional sensor 111 is the information transmitted from the wireless transmitter whose receivable range is specified. A position / orientation estimation device 101, a distance sensor 110, and an additional sensor 111 are installed on the moving body 201.

It is assumed that when the position detection system is activated, the distance sensor 110 detects the peripheral shape 205 as a result of the building wall surface 203 and the obstacle 204 blocking the detection area 202 of the distance sensor 110.

When the self-position is estimated based on the peripheral shape 205, the self-position cannot be determined because there are a plurality of matching positions in the building wall surface 203. Therefore, the additional sensor 111 acquires data that can narrow down the self-position.

It is assumed that a plurality of transmitters 206 are installed in the building, and the data transferable area 207 transmitted from each transmitter 206 exists in a circular shape. It is assumed that individual information is transmitted from each transmitter 206, and the range in which the individual information can be acquired is linked to the map 109 owned by the position / orientation estimation device 101.

By receiving the individual information by the additional sensor 111, it becomes possible to determine which circular acquireable area 207 exists in the received individual information. By combining this individual information with the information of the peripheral shape 205, it is possible to narrow down if the self-position is at the position of the moving body 201 in FIGS. 2A and 2B.

The order of narrowing down the self-position based on the individual information received by the additional sensor 111 and estimating the self-position from the surrounding conditions acquired by the distance sensor 110 does not matter.

For example, after narrowing down the self-position candidates based on the information received by the additional sensor 111, the self-position applicable to the narrowed down candidates may be estimated based on the information obtained by the distance sensor 110 (usage mode of FIG. 2A).
Alternatively, after self-position estimation from the surrounding conditions acquired by the distance sensor 110, when there are a plurality of estimated positions, narrowing down may be performed based on the information acquired by the additional sensor 111 (usage mode of FIG. 2B). ).

With reference to FIGS. 2A and 3, after narrowing down the self-position candidates based on the information received by the additional sensor 111, the usage mode in which the self-position applicable to the narrowed down candidates is estimated based on the information obtained by the distance sensor 110. explain.

As shown in FIG. 3, when the self-position estimation is started, data is acquired by the additional sensor 111 in S301. After the data is acquired, the area in which the data acquired by the additional sensor 111 can be acquired is narrowed down from the map (map data) 109 existing in the position / orientation estimation device 101 in S302, and the corresponding data can be received in the map data 109. The area inside is the target of self-position search.

If the data cannot be acquired by the additional sensor 111, the range in which the data cannot be received is the self-position search target. After narrowing down the self-position candidates, S303 acquires the data of the distance sensor 110, and S304 reads the map data for self-position search.

Based on the measurement data of the distance sensor 110 acquired in S303, the search target area in the map 109 read in S305 is compared to see if there is a matching position.

It is determined whether or not there is a matching position on the map 109 read in S306, and if there is a matching position, it is saved as a self-position candidate in S307.

If it is determined in S306 that the matching position is not on the searched map 109, the process of S307 is omitted, the read map 109 is stored in S308, and the map data for which the self-position candidate is searched is searched. Close.

It is determined in S309 whether all the candidate map data 109s have been searched, and if all the candidate map 109s have not been searched, another map 109 that is a candidate in S310 is displayed. Read, return to the process of S305, and continue the process.

When it is determined that all the map data 109 candidates in S309 have been searched, the most probable self-position is narrowed down from the self-position candidates saved in S307 in S311.

In S311, when a plurality of self-position candidates exist in S306, the self is based on information such as the degree of agreement between the map 109 when determined as the self-position candidate and the surrounding shape measured by the distance sensor 110. Narrow down the position.

If there is only one self-position candidate, it is determined as the self-position as it is. If the self-position candidate does not exist, it is determined that the self-position candidate does not exist in the map 109. Regarding the search process of the self-position candidate on the map data 109 of S304 to S310, the process may be executed in parallel for the plurality of map data 109.

When narrowing down the self-position search target first, the self-position search target is narrowed down and then the self-position is estimated. Therefore, the target area to be compared when estimating the self-position is reduced, and the self in a shorter time. Enables position estimation. In this case, it is also applied when the same information can be obtained from the maps 109 located in the vicinity of the plurality of maps 109 existing in the position / orientation estimation device.

In this case, "located in the periphery" means that the spaces indicated by two or more different maps 109 stored in the position / orientation estimation device exist at adjacent positions in the real space, or at positions adjacent to each other in the vertical direction. Indicates the case where it exists in.

After narrowing down the map 109 and self-position candidates for which detailed self-position estimation should be performed based on the information obtained by the additional sensor 111, detailed self-position estimation is performed by the distance sensor 110. As a result, it is possible to reduce the number of objects to be compared as compared with the case where the self-position estimation is performed using only the distance sensor 110. As a result, the amount of calculation can be suppressed and the estimation can be performed in a shorter time.

With reference to FIG. 4, a case where they exist at positions adjacent to each other in the vertical direction will be described as an example.
As shown in FIG. 4, there is a moving body 401 equipped with a position / orientation estimation device 101, a distance sensor 110, and an additional sensor 111, and a region that can be measured by the distance sensor 110 is defined as a measurable region 402.

There is a map 403 on the first floor and a map 404 on the second floor, and the signal transmitted by the transmitter 206 installed on the first floor passes through the ceiling on the first floor and the floor on the second floor, and is part of the first and second floors. It is possible to receive at. It is assumed that the receivable area is associated with the map 403 on the first floor and the map 404 on the second floor as 207.

Among the plurality of transmitters 206, those transmitting the signal received by the additional sensor 111 are shown in FIG. Since the signal transmitted by the transmitter 206 can be received by the additional sensor 111, the position where the moving body 401 exists is within the circular region of the receivable region 207. Therefore, the self-position search candidates are narrowed down to the receivable area 207 in the map existing in the position / orientation estimation device.

After this narrowing down, self-position estimation is performed based on the peripheral shape in the measurable area 402, and it is found that the position and posture in which the moving body 401 exists is the position and posture of the moving body 401 on the map 403.

Further, when narrowing down the self-position candidates using the information obtained by the additional sensor 111, if the self-position candidates having similar peripheral shapes measured by the distance sensor 110 are narrowed down to one, the additional sensor is used. Accurate self-position estimation can be performed by using 111 together.

With reference to FIGS. 2B and 5, after self-position estimation from the surrounding conditions acquired by the distance sensor 110, when there are a plurality of estimated positions, the use is performed by narrowing down by the information acquired by the additional sensor 111. Aspects will be described.

As shown in FIG. 5, when the self-position estimation is started, the measurement data of the distance sensor 110 is acquired in S501. After acquiring the measurement data of the distance sensor 110, the map 109 owned by the position / orientation estimation device 101 is read by S502.

Based on the measurement data of the distance sensor 110 acquired in S501, the map 109 read in S503 is compared to see if there is a matching position.

It is determined whether or not there is a matching position on the map 109 read in S504, and if there is a matching position, it is saved as a self-position candidate in S505.

If it is determined that the matching position is not on the searched map 109 in S504, the process of S505 is omitted, the read map 109 is stored in S506, and the map 109 searched for the self-position candidate is stored. Close.

In S507, it is determined whether or not all the maps 109 stored in the self-position estimation device have been searched. If all the maps 109 have not been searched, another map 109 is read in S508, the process returns to S503, and the process is continued.

When it is determined in S507 that all the maps 109 have been searched, it is determined in S509 whether or not there are two or more candidates for the self-position. When there are two or more self-position candidates, the data of the additional sensor 111 is acquired in S510, and the self-position candidates are narrowed down based on the data acquired in S511.

When the self-position candidate cannot be narrowed down only by the data of the additional sensor 111, the self-position is narrowed down based on the information such as the degree of coincidence with the surrounding shape measured by the distance sensor 110.

When it is determined in S509 that there are no two or more self-position candidates, and there is only one self-position candidate, it is determined as the self-position as it is. If the self-position candidate does not exist, it is determined that the self-position candidate does not exist in the map 109.

Regarding the search process of the self-position candidate on the map 109 of S502 to S508, the process may be executed in parallel for a plurality of maps 109.
When narrowing down when there are a plurality of candidates after self-position estimation, if the self-position can be estimated only by the information of the distance sensor 110, the data acquired by the additional sensor 111 is processed only when necessary. conduct. As a result, it is possible to omit the processing of the data acquired by the additional sensor 111 when it is unnecessary, and it is possible to suppress the execution of the additional processing.

In the position detection system shown in the first embodiment, it is assumed that a wireless LAN slave unit is used as the additional sensor 111. The wireless LAN base unit has an SSID (Service Set Identity), a MAC address (Media Access Control addless), and an identifier called an IP address (Internet Protocol Addless).

By using any of these, it is possible to narrow down the candidates for the self-position based on the information available on the additional sensor 111. One or more of the SSID, MAC address, and IP address information of the wireless LAN base unit is linked to the map 109 or a part of the map 109 owned by the position / orientation estimation device 101.

When the information is obtained by the additional sensor 111, the self-position candidate search target area in the map 109 or the map 109 associated with the obtained information is narrowed down. Depending on the hardware or implementation configuration, linked data may be prepared on a connectable network and referenced as necessary.

In the position detection system shown in the first embodiment, it is assumed that a Bluetooth (registered trademark) compatible device is used as the additional sensor 111. The Bluetooth compatible device has a BD address (Bluetooth Device address) and a device name. By using any of these, it is possible to narrow down the candidates for the self-position based on the information available on the additional sensor 111.

Information on either or both of the BD address and the device name is linked to a part of the map 109 or the map 109 possessed by the position / orientation estimation device 101. When the information is obtained by the additional sensor 111, the self-position candidate search target area in the map 109 or the map 109 associated with the obtained information is narrowed down.

Depending on the hardware or implementation configuration, linked data may be prepared on a connectable network and referenced as necessary.

In the position detection system shown in the first embodiment, it is assumed that a GNSS (Global Navigation Satellite System) receiver is used as the additional sensor 111.

The GNSS receiver identifies its own position based on the signal transmitted from the positioning satellite, and outputs position information such as its latitude, longitude, and altitude. Since an error may be seen in the position information depending on the receiving position and environment, the map 109 existing in the position / orientation estimation device 101 is associated with the latitude and longitude ranges in consideration of the error.

When the received latitude and longitude are within the range set for each map, it can be applied by targeting the map as a self-position candidate search target. The magnitude of the error depends on the environment inside and outside the map 109 and the placement of the positioning satellites. The error range can be narrowed down more accurately by setting it according to the building materials and structure of the building targeted by Map 109 and the surrounding environment.

Depending on the hardware or implementation configuration, linked data may be prepared on a connectable network and referenced as necessary.

In the position detection system shown in the first embodiment, it is assumed that a receiver capable of receiving a signal conforming to the IMES message specifications is used as the additional sensor 111.
When this receiver receives a signal, it outputs latitude, longitude, altitude information, and the like. Therefore, it can be used in the same manner as in the sixth embodiment, but the information output by the receiver is information such as the latitude, longitude, and number of floors of the position where the transmitter is attached, which is transmitted from the transmitter. Therefore, it is not necessary to perform the positioning calculation on the receiver side, and it is possible to reduce the setting of the error range even when the receiver is used indoors.

Depending on the hardware or implementation configuration, linked data may be prepared on a connectable network and referenced as necessary.

It is assumed that a camera is used as the additional sensor 111 in the position detection system shown in the first embodiment.
A barcode, QR code (registered trademark), or a mark such as a characteristic pattern or shape that can be recognized by the camera is installed at a position in the building that can be photographed by the camera attached to the position detection system. The arrangement and contents of the mark are linked to the map 109 or a part of the map 109 possessed by the position / orientation estimation device 101.

When the information is obtained by the additional sensor 111, the self-position search target area in the map 109 associated with the obtained information is narrowed down. When a barcode or QR code is used as a mark, information on a map 109 or a target area to be directly referred to may be embedded in the barcode or QR code itself.

Depending on the hardware or implementation configuration, linked data may be prepared on a connectable network and referenced as necessary.

The additional sensor 111 may be used not only in one type but also in combination of a plurality of types. By processing the data obtained from the plurality of additional sensors 111 in a complex manner, it is possible to estimate the accurate self-position in a short time. As an example, consider the case where a slave unit for wireless LAN and a Bluetooth receiver capable of receiving a beacon are used as the additional sensor 111.

When both of these are received at the same time, the self-position may be estimated and calculated from the surrounding conditions acquired by the distance sensor 110 only in the range in which both of them can be received at the same time on the map 109. Therefore, it is possible to narrow down the candidate locations for position estimation as compared with the case where a single additional sensor 111 is used.

On the contrary, when only one of the wireless LAN slave unit and the Bluetooth receiver receives data, or when both additional sensors 111 do not receive data, the respective conditions are satisfied. Only the candidate locations for position estimation are to be confirmed. Therefore, it is possible to narrow down the candidate locations for position estimation.

101 Position / Attitude Estimator 102 Arithmetic Logic Unit 103 Memory 104 Communication Device 105 Storage Device 106 Position / Attitude Estimate Program 107 Communication Program 108 Sensor Control Program 109 Map 201 Moving Object

Claims (7)

A moving body position detection system that estimates its own position within the motion area in which the moving body moves.
The moving body is
The first sensor that measures the surrounding conditions and
A second sensor that sets a plurality of data acquisition areas that can be distinguished from each other in the operating area,
It has a position estimation unit that estimates the self-position, and
The position estimation unit
The plurality of data acquisition areas set by the second sensor and the map data in the operation area are linked in advance.
Wherein said collating map data based the first of the measurement data measured by sensors and the second of said data acquisition area set by the sensor, the self-position estimating performs narrowing of the self-position ,
The position estimation unit
As a result of collating the map data based on the measurement data measured by the first sensor, when a plurality of candidates for the self-position exist in the operating region, the data set by the second sensor. The map data is collated based on the acquisition area, and the final self-position is narrowed down from the plurality of candidates for the self-position.
As a result of collating the map data based on the measurement data measured by the first sensor, if there is only one candidate for the self-position in the operating region, the second sensor is not used. In addition, a position detection system characterized in that the final self-position is estimated based on the measurement data measured by the first sensor. The position estimation unit
When the final self-position is narrowed down from the plurality of candidates for the self-position, if the final self-position cannot be narrowed down by using the second sensor, the first sensor The position detection system according to claim 1, wherein the final self-position is narrowed down by using. The second sensor is
The first aspect of claim 1, wherein the receiver comprises a receiver that receives information transmitted from a transmitter for which a receivable range is specified and sets a plurality of the data acquisition areas within the operating area. Position detection system. A plurality of the transmitters are installed in the operating area, and individual information is transmitted from each of the plurality of transmitters.
The range in which the individual information can be acquired is previously linked to the map data as the data acquisition area.
The claim is characterized in that, by receiving the individual information by the receiver, the data acquisition area in which the moving body exists is specified from the plurality of data acquisition areas, and the self-position is narrowed down. 3. The position detection system according to 3. The transmitter is a wireless LAN base unit and
The receiver is a wireless LAN slave unit and
The position detection system according to claim 4 , wherein an identifier is assigned to the wireless LAN base unit as the individual information. The position detection system according to claim 1, wherein the first sensor is composed of a distance sensor that irradiates a laser beam and measures the surrounding situation. The moving body is
While estimating the self-position with respect to a plurality of obstacles arranged in the operating area, the obstacles are avoided and moved.
The distance sensor is
The position detection system according to claim 6 , further comprising irradiating the obstacle with the laser beam and measuring the surrounding situation by receiving the reflected light from the obstacle.

Images