WO2019194079A1

WO2019194079A1 - Position estimation system, moving body comprising said position estimation system, and computer program

Info

Publication number: WO2019194079A1
Application number: PCT/JP2019/013741
Authority: WO
Inventors: 慎治鈴木; 佐伯　哲夫
Original assignee: 日本電産株式会社
Priority date: 2018-04-02
Filing date: 2019-03-28
Publication date: 2019-10-10
Also published as: CN111971633B; CN111971633A

Abstract

[Problem] To provide a self-estimation system capable of self-position estimation even if a portion of a map does not represent the actual environment. [Solution] A position estimation system (115) according to this disclosure is for a moving body and is used while being connected to an external sensor for repeatedly scanning an environment and outputting sensor data for each scan. The position estimation system (115) comprises a processor (106), a first memory (104) for storing an environment map that has been prepared beforehand, and a second memory (107) for storing a computer program for operating the processor. The processor carries out (A) first position estimation processing for generating first estimated values for the position and orientation of the moving body on the basis of the results of comparing the environment map and the sensor data, (B) second position estimation processing for using the sensor data to generate a reference map for the surrounding area while generating second estimated values for the position and orientation of the moving body on the basis of the results of comparing the reference map and the sensor data, and (C) processing in which the first estimated values or second estimated values are selected and the selected estimated values are output as the selected estimated values for the position and orientation of the moving body.

Description

POSITION ESTIMATION SYSTEM, MOBILE BODY HAVING THE POSITION ESTIMATION SYSTEM, AND COMPUTER PROGRAM

The present disclosure relates to a position estimation system and a moving object including the position estimation system. The present disclosure also relates to a computer program used for position estimation.

Development of a mobile body that can move autonomously such as an automatic guided vehicle (automatic transport cart) and a mobile robot is in progress. *

Japanese Patent Application Laid-Open No. 2008-250905 discloses a mobile robot that performs self-position estimation by matching sensor data acquired from a laser range finder with a map prepared in advance.

JP 2008-250905 A

The environment may change after the map is created. For example, layouts are often temporarily changed in factories and distribution warehouses. In addition, a free space may be temporarily reduced by placing a load, a device, or another moving body in a place (free space) where the moving body can pass on the map. If matching is performed using an environment map that does not reflect the actual environment, a situation in which the self-position cannot be estimated may occur. *

Embodiments of the present disclosure provide a self-estimation system and a moving body that can estimate a self-position even when a part of a map does not reflect an actual environment, and a computer program used for the self-estimation.

The position estimation system of the present disclosure is a position estimation system for a moving body that is used in connection with an external sensor that repeatedly scans an environment and outputs sensor data for each scan in a non-limiting exemplary embodiment. And a first memory for storing an environmental map prepared in advance, and a second memory for storing a computer program for operating the processor. The at least one processor generates a first estimated value of the position and orientation of the moving body based on a result of collation between the environmental map and the sensor data in accordance with a command of the computer program. A position estimation process; and (B) generating a surrounding reference map using the sensor data, and calculating a second estimated value of the position and orientation of the moving body based on a result of matching the reference map with the sensor data. A second position estimation process to be generated; and (C) one of the first estimated value and the second estimated value is selected, and the selected estimated value is selected as the selected position and orientation of the moving object. Perform output as a value. *

In a non-limiting exemplary embodiment, a moving body of the present disclosure includes the position estimation system described above, the external sensor, and a driving device for movement. *

The computer program of the present disclosure is a computer program used in any one of the above-described position estimation systems in a non-limiting exemplary embodiment.

According to the embodiment of the present disclosure, it is possible to estimate the self position even when a part of the map does not reflect the actual environment.

FIG. 1 is a diagram illustrating a configuration of an embodiment of a moving object according to the present disclosure. FIG. 2 is a plan layout diagram schematically showing an example of an environment in which a moving body moves. FIG. 3 is a diagram showing an environment map of the environment shown in FIG. FIG. 4A is a diagram schematically illustrating an example of scan data SD (t) acquired by the external sensor at time t. FIG. 4B is a diagram schematically illustrating an example of scan data SD (t + Δt) acquired by the external sensor at time t + Δt. FIG. 4C is a diagram schematically illustrating a state in which the scan data SD (t + Δt) is matched with the scan data SD (t). FIG. 5 is a diagram schematically illustrating how the point group constituting the scan data rotates and translates from the initial position and approaches the point group of the environment map. FIG. 6 is a diagram illustrating the position and orientation of the scan data after the rigid body conversion. FIG. 7A is a diagram schematically illustrating a state in which scan data is acquired from an external sensor, a reference map is created from the scan data, and then newly acquired scan data is matched with the reference map. FIG. 7B is a diagram schematically illustrating a reference map that is updated by adding newly acquired scan data to the reference map of FIG. 7A. FIG. 7C is a diagram schematically illustrating a reference map that is updated by adding newly acquired scan data to the reference map of FIG. 7B. FIG. 8A is a diagram schematically illustrating an example of scan data SD (t) acquired by the external sensor at time t. FIG. 8B is a diagram schematically illustrating a state when matching of the scan data SD (t) with the environment map M is started. FIG. 8C is a diagram schematically illustrating a state where the matching of the scan data SD (t) with the environment map M is completed. FIG. 9 is a diagram schematically illustrating the history of the position and orientation of the moving body obtained in the past and the predicted values of the current position and orientation. FIG. 10 is a flowchart illustrating a part of the operation of the position estimation device according to the embodiment of the present disclosure. FIG. 11A is a diagram illustrating an example in which the amount of change in the first estimated value due to the first position estimation process (offline SLAM) varies. FIG. 11B is a diagram illustrating an example in which the difference between the first estimated value obtained by the first position estimating process (offline SLAM) and the measured value obtained by the sensor varies. FIG. 11C is a diagram illustrating an example in which the reliability of the first estimated value by the first position estimation process (offline SLAM) and the reliability of the second estimated value by the second position estimation process (online SLAM) vary. FIG. 12 is a flowchart illustrating a part of the operation of the position estimation device according to the embodiment of the present disclosure. FIG. 13 is a flowchart illustrating an example of second position estimation processing (online SLAM) of the position estimation device according to the embodiment of the present disclosure. FIG. 14 is a diagram illustrating an overview of a control system that controls traveling of each AGV according to the present disclosure. FIG. 15 is a perspective view showing an example of an environment where AGVs exist. FIG. 16 is a perspective view showing the AGV and the towing cart before being connected. FIG. 17 is a perspective view showing the AGV and the traction cart connected to each other. FIG. 18 is an external view of an exemplary AGV according to the present embodiment. FIG. 19A is a diagram illustrating a first hardware configuration example of AGV. FIG. 19B is a diagram illustrating a second hardware configuration example of AGV. FIG. 20 is a diagram illustrating a hardware configuration example of the operation management apparatus.

<Terminology> “Automated guided vehicle” (AGV) means a trackless vehicle in which a load is loaded on the main body manually or automatically, automatically travels to a designated place, and unloads manually or automatically. “Automated guided vehicle” includes automatic guided vehicles and automatic forklifts. *

The term “unmanned” means that no person is required to steer the vehicle, and it does not exclude that the automated guided vehicle transports “person (for example, a person who loads and unloads luggage)”. *

An “unmanned towing vehicle” is a trackless vehicle that automatically pulls a cart that loads and unloads luggage manually or automatically travels to a designated location. *

An “unmanned forklift” is a trackless vehicle that includes a mast that moves up and down a load transfer fork, automatically transfers the load to the fork, etc., automatically travels to a designated location, and performs automatic cargo handling work. *

A “trackless vehicle” is a vehicle that includes wheels and an electric motor or engine that rotates the wheels. *

The “moving body” is a device that carries a person or a load and moves, and includes a driving device such as a wheel, a biped or multi-legged walking device, and a propeller that generate driving force (traction) for movement. The term “mobile body” in the present disclosure includes not only a narrow automatic guided vehicle but also a mobile robot, a service robot, and a drone. *

The “automatic traveling” includes traveling based on a command of a computer operation management system to which the automatic guided vehicle is connected by communication, and autonomous traveling by a control device included in the automatic guided vehicle. Autonomous traveling includes not only traveling where the automated guided vehicle travels to a destination along a predetermined route, but also traveling following a tracking target. Moreover, the automatic guided vehicle may temporarily perform manual travel based on an instruction from the worker. “Automatic travel” generally includes both “guided” travel and “guideless” travel, but in the present disclosure, it means “guideless” travel. *

The “guide type” is a system in which a derivative is installed continuously or intermittently and the guided vehicle is guided using the derivative. *

The “guideless type” is a method of guiding without installing a derivative. An automatic guided vehicle according to an embodiment of the present disclosure includes a position estimation device and can travel in a guideless manner. *

The “position estimation device” is a device that estimates a self-position on an environmental map based on sensor data acquired by an external sensor such as a laser range finder. *

An “external sensor” is a sensor that senses an external state of a moving body. Examples of the external sensor include a laser range finder (also referred to as a range sensor), a camera (or an image sensor), a LIDAR (Light Detection and Ranging), a millimeter wave radar, an ultrasonic sensor, and a magnetic sensor. *

The “inner world sensor” is a sensor that senses the state inside the moving body. Examples of the internal sensor include a rotary encoder (hereinafter sometimes simply referred to as “encoder”), an acceleration sensor, and an angular acceleration sensor (for example, a gyro sensor). *

“SLAM” is an abbreviation of “Simultaneous” “Localization” and “Mapping”, and means that self-location estimation and environmental map creation are performed simultaneously. *

<Basic Configuration of Moving Body in Present Disclosure> Reference is made to FIG. The mobile body 10 of the present disclosure includes an external sensor 102 that scans an environment and periodically outputs scan data in the exemplary embodiment shown in FIG. 1. A typical example of the external sensor 102 is a laser range finder (LRF). The LRF periodically emits, for example, an infrared or visible laser beam to the surroundings to scan the surrounding environment. The laser beam is reflected by a surface such as a structure such as a wall or a pillar or an object placed on the floor. The LRF receives the reflected light of the laser beam, calculates the distance to each reflection point, and outputs measurement result data indicating the position of each reflection point. The direction and distance of the reflected light are reflected at the position of each reflection point. The measurement result data (scan data) may be referred to as “environmental measurement data” or “sensor data”. *

The environment scan by the external sensor 102 is performed, for example, on an environment in a range of 135 degrees to the left and right (total 270 degrees) with respect to the front of the external sensor 102. Specifically, a pulsed laser beam is emitted while changing the direction at predetermined step angles in the horizontal plane, and the reflected light of each laser beam is detected to measure the distance. If the step angle is 0.3 degree, measurement data of the distance to the reflection point in the direction determined by the angle corresponding to the total of 901 steps can be obtained. In this example, the scanning of the surrounding space performed by the external sensor 102 is substantially parallel to the floor surface and is planar (two-dimensional). However, the external sensor may perform a three-dimensional scan. *

A typical example of the scan data can be expressed by the position coordinates of each point constituting a point cloud acquired for each scan. The position coordinates of the points are defined by a local coordinate system that moves with the moving body 10. Such a local coordinate system may be referred to as a mobile coordinate system or a sensor coordinate system. In the present disclosure, the origin of the local coordinate system fixed to the moving body 10 is defined as the “position” of the moving body 10, and the orientation of the local coordinate system is defined as the “posture” of the moving body 10. Hereinafter, the position and orientation may be collectively referred to as “pose”. In addition, for the sake of simplicity, “pose” may be simply referred to as “position”.

When the scan data is displayed in a polar coordinate system, the scan data may be constituted by a numerical set indicating the position of each point by “direction” and “distance” from the origin in the local coordinate system. The polar coordinate system representation may be converted to an orthogonal coordinate system representation. In the following description, for the sake of simplicity, it is assumed that the scan data output from the external sensor is displayed in an orthogonal coordinate system. *

The moving body 10 includes a storage device (first memory) 104 that stores an environment map, and a position estimation system 115. The environment map is prepared by the mobile object 10 or another map creation device and stored in the storage device 104. *

The position estimation system 115 is used by being connected to the external sensor 102, and includes a processor 106 and a memory (second memory) 107 that stores a computer program for controlling the operation of the processor. Although FIG. 1 shows one processor 106 and one memory 107, the number of processors 106 and memory 107 may be two or three or more, respectively. *

The position estimation system 115 performs matching between the scan data acquired from the external sensor 102 and the environment map read from the storage device 104, and estimates the position and orientation of the moving body 10, that is, the pose. This matching is called pattern matching or scan matching and can be performed according to various algorithms. A typical example of the matching algorithm is an Iterative Closest Point (ICP: iterative nearest neighbor) algorithm. *

As will be described later, the position estimation system 115 can create an environmental map by matching and connecting a plurality of scan data output from the external sensor 102 by matching. *

The position estimation system 115 in the embodiment of the present disclosure is realized by the processor 106 and the memory 107 that stores a computer program that operates the processor 106. The processor 106 performs the following operations in accordance with instructions from the computer program. *

(A) First position estimation process (offline SLAM): Generates a first estimated value of the position and posture of the external sensor 102 (that is, the position and posture of the moving body 10) based on the result of collation between the environmental map and the sensor data. . *

(B) Second position estimation process (online SLAM): While generating a surrounding map (reference map) using sensor data, the position and orientation of the moving body 10 based on the result of matching the reference map with the sensor data The second estimated value is generated. *

(C) One of the first estimated value and the second estimated value is selected, and the selected estimated value is output as the “selected estimated value” of the position and orientation of the moving body 10. *

In the embodiment of the present disclosure, the processor 106 executes the first position estimation process and the second position estimation process simultaneously or in parallel. For example, the processor 106 may include a first processor part that executes a first position estimation process and a second processor part that executes a second position estimation process. Further, one processor 106 may alternately perform operations necessary for the first position estimation process and the second position estimation process. *

In the process (C), when the processor 106 outputs the first estimated value as the selected estimated value and any of the following events occurs, the processor 106 outputs the second estimated value instead of the first estimated value. Is output as the selected estimated value. *

The reliability of the event / first estimated value was calculated, and this reliability was lower than the threshold. *

The reliability of the first estimated value (first reliability) and the reliability of the second estimated value (second reliability) were calculated, and the first reliability was lower than the second reliability. *

The current first estimated value has changed from the past first estimated value (time-series data) beyond a predetermined range. *

The difference between the measured value (odometry value) of the position and / or orientation acquired from the internal sensor and the first estimated value exceeded the threshold value. *

The calculation for determining the first estimated value could not be completed within a predetermined time (the matching error did not converge to a sufficiently small level within the predetermined time). *

The reliability can be expressed quantitatively by, for example, a final error (“position shift amount” or “match rate”) of ICP matching described later. *

Each of the above events occurs when the environment map used in offline SLAM does not accurately reflect the current environment. For example, when a layout is temporarily changed in a factory or a distribution warehouse, a load, a device, or another moving body is placed in a place (free space) where the moving body can pass on the environmental map. In spite of such an environmental change, if matching is performed using the previous environmental map as it is, the above-mentioned phenomenon is likely to occur. *

In the embodiment of the present disclosure, position estimation is performed by the first position estimation process (offline SLAM), while position estimation by the second position estimation process (online SLAM) is also performed in the background. For this reason, when a shift | offset | difference arises between an environmental map and real environment, the estimated value (2nd estimated value) by (online SLAM) can be output rapidly and can be utilized. *

The second position estimation process (online SLAM) can be executed by the following process, for example. *

(1) Scan data is acquired from the external sensor 102, and a reference map (local map) is created from the scan data. *

(2) When the scan data is newly acquired from the external sensor 102, the position and orientation of the moving body 10 on the reference map are estimated by matching the newly acquired latest scan data with the reference map. Update the reference map by adding scan data to the reference map. *

(3) The reference map is reset by deleting a part other than the part including the latest scan data from the reference map updated a plurality of times. *

Also if necessary. The following processing may be additionally executed. *

(4) When resetting, the environment map is updated based on the reference map updated multiple times before resetting (online map update). *

By updating the environment map itself, the first estimated value obtained by the first position estimation process (offline SLAM) can be obtained without the above-described phenomenon even when the environment change state (for example, layout change) continues. Can be used as the selected estimated value. *

In the illustrated example, the moving body 10 further includes a drive device 108, an automatic travel control device 110, and a communication circuit 112. The driving device 108 is a device that generates a driving force for the moving body 10 to move. Examples of drive device 108 include a biped or multi-legged walking device that is operated by a wheel (drive wheel) that is rotated by an electric motor or engine, a motor or other actuator. The wheel may be an omnidirectional wheel such as a Mecanum wheel. Further, the moving body 10 may be a moving body that moves in the air or underwater, or a hovercraft. In this case, the driving device 108 includes a propeller that is rotated by a motor. *

The automatic travel control device 110 operates the driving device 108 to control the moving conditions (speed, acceleration, moving direction, etc.) of the moving body 10. The automatic traveling control device 110 may move the moving body 10 along a predetermined traveling route, or may move it according to a command given from the outside. The position estimation system 115 calculates an estimated value of the position and orientation of the moving body 10 while the moving body 10 is moving or stopped. The automatic traveling control device 110 controls traveling of the moving body 10 with reference to the estimated value. *

The position estimation system 115 and the automatic travel control device 110 may be collectively referred to as a travel control device 120. The automatic traveling control device 110 may be configured by the position estimation system 115 and the processor 106 described above and a memory 107 storing a computer program for controlling the operation of the processor 106. Such a processor 106 and memory 107 can be realized by one or a plurality of semiconductor integrated circuits. *

The communication circuit 112 is a circuit in which the mobile unit 10 is connected to a communication network including an external management device, another mobile unit, or an operator's mobile terminal device, and exchanges data and / or commands. *

<Environment Map> FIG. 2 is a plan layout diagram schematically showing an example of an environment 200 in which the moving body 10 moves. The environment 200 is part of a wider environment. In FIG. 2, a thick straight line indicates a fixed wall 202 of a building, for example. *

FIG. 3 is a diagram showing a map (environment map M) of the environment 200 shown in FIG. Each dot 204 in the figure corresponds to each point of the point group constituting the environment map M. In the present disclosure, the point cloud of the environment map M may be referred to as a “reference point cloud”, and the scan data point cloud may be referred to as a “data point cloud” or a “source point cloud”. Matching is, for example, aligning scan data (data point group) with respect to an environmental map (reference point group) having a fixed position. When matching using the ICP algorithm, specifically, a pair of corresponding points is selected between the reference point group and the data point group, and the distance (error) between the points constituting each pair is minimized. The position and orientation of the data point group are adjusted. *

In FIG. 3, the dots 204 are arranged at equal intervals on a plurality of line segments for simplicity. The point cloud in the actual environment map M may have a more complicated arrangement pattern. The environment map M is not limited to a point cloud map, and may be a map having a straight line or a curve as a constituent element, or may be an occupied grid map. That is, it is only necessary that the environment map M has a structure capable of performing matching between the scan data and the environment map M. In the case of an occupied grid map, Monte Carlo position estimation can be performed. Hereinafter, although an embodiment of the present disclosure will be described by taking matching by the ICP algorithm as an example, the embodiment of the present disclosure is not limited to this example. *

When the moving body 10 is at each of the position PA, position PB, and position PC shown in FIG. 3, the scan data acquired by the external sensor 102 of the moving body 10 has a different point cloud arrangement. If the moving time from the position PA to the position PC via the position PB is sufficiently long compared to the scanning period of the external sensor 102, that is, if the moving of the moving body 10 is slow, the time axis The two adjacent scan data above are very similar. However, when the moving body 10 moves remarkably fast, the two adjacent scan data on the time axis may be greatly different. *

As described above, when the latest scan data among the scan data sequentially output from the external sensor 102 is similar to the scan data immediately before, the matching is relatively easy and the reliability is high in a short time. Matching is expected. However, when the moving speed of the moving body 10 is relatively high, there is a possibility that the latest scan data is not similar to the immediately preceding scan data, and the time required for matching becomes long, or matching within a predetermined time May not complete. *

<Matching at Map Creation> In the embodiment of the present disclosure, map creation can be performed online or offline. For this reason, as for the principle of map creation, the following description can be applied to both offline SLAM and online SLAM. However, in the case of offline SLAM, there is no particular restriction on the time required for creating a map using scan data, and it is possible to perform matching with less error by taking time. *

FIG. 4A is a diagram schematically illustrating an example of scan data SD (t) acquired by the external sensor 102 at time t. The scan data SD (t)
It is displayed in a sensor coordinate system whose position and orientation change with zero. The scan data SD (t) is represented by a UV coordinate system in which the front surface of the external sensor 102 is the V axis, and the direction rotated 90 ° clockwise from the V axis is the U axis. The moving body 10, more precisely, the external sensor 102 is located at the origin of the UV coordinate system. In the present disclosure, when the moving body 10 moves forward, the moving body 10 advances in front of the external sensor 102, that is, in the direction of the V-axis. For easy understanding, the points constituting the scan data SD (t) are indicated by black circles.

In this specification, the period at which the position estimation system 115 acquires scan data from the external sensor 102 is denoted by Δt. Δt is, for example, 200 milliseconds. When the moving body 10 is moving, the content of the scan data periodically acquired from the external sensor 102 can change. *

FIG. 4B is a diagram schematically illustrating an example of scan data SD (t + Δt) acquired by the external sensor 102 at time t + Δt. For easy understanding, the points constituting the scan data SD (t + Δt) are indicated by white circles. *

When Δt is, for example, 200 milliseconds, if the moving body 10 moves at a speed of 1 mail per second, the moving body 10 moves about 20 centimeters during Δt. Normally, the environment of the moving body 10 does not change greatly due to movement of about 20 centimeters. Therefore, there is a wide overlap between the environment scanned by the external sensor 102 at time t + Δt and the environment scanned at time t. Is included. Therefore, many corresponding points are included between the point group of the scan data SD (t) and the point group of the scan data SD (t + Δt). *

FIG. 4C schematically shows a state where matching between the scan data SD (t) and the scan data SD (t + Δt) is completed. In this example, alignment is performed so that the scan data SD (t + Δt) is aligned with the scan data SD (t). The moving body 10 at time t is located at the origin of the UV coordinate system in FIG. 4C, and the moving body 10 at time t + Δt is at a position moved from the origin of the UV coordinate system. By matching two pieces of scan data, the arrangement relationship of the other local coordinate system with respect to one local coordinate system is obtained. *

In this way, a local environment map can be created by connecting a plurality of scan data SD (t), SD (t + Δt),..., SD (t + N × Δt) acquired periodically. Here, N is an integer of 1 or more. By synthesizing a plurality of local environment maps, an entire environment map can be obtained. In the case of online SLAM, the entire environment map that has been synthesized is completed. In the present disclosure, a “local environment map” created by a moving body while moving is referred to as a “reference map” and may be distinguished from an “environment map” used in offline SLAM. *

FIG. 5 is a diagram schematically showing how the point group constituting the scan data at time t rotates and translates from the initial position and approaches the point group on the map. The coordinate value of the kth point (k = 1, 2,..., K−1, K) of the K points constituting the point group of the scan data at time t is represented by Z _{t, k} . Let m _{k be} the coordinate value of the point on the map corresponding to. At this time, the error of the corresponding points in the two point groups can be evaluated by using Σ (Z _{t, k} −m _k ) ² that is the sum of squares of the errors calculated for the K corresponding points as a cost function. Rotational and translational rigid body transformations are determined so as to reduce Σ (Z _{t, k} −m _k ) ² . The rigid transformation is defined by a transformation matrix (homogeneous transformation matrix) that includes a rotation angle and a translation vector as parameters.

FIG. 6 is a diagram illustrating the position and orientation of the scan data after the rigid body conversion. In the example shown in FIG. 6, matching between the scan data and the map is not completed, and a large error (positional deviation) still exists between the two point groups. In order to reduce this displacement, rigid body transformation is further performed. Thus, when the error becomes smaller than a predetermined value, matching is completed. *

<Creation of Reference Map> Hereinafter, creation of a reference map performed during online SLAM will be described. *

FIG. 7A is a diagram schematically illustrating a state where matching between the latest scan data SD (b) newly acquired and the scan data SD (a) acquired last time is completed. In FIG. 7A, a black circle point group represents the previous scan data, and a white circle point group represents the latest scan data. FIG. 7A shows the position a of the moving body 10 when the previous scan data is acquired and the position b of the moving body 10 when the latest scan data is acquired. *

In this example, the scan data SD (a) acquired last time constitutes a “reference map RM”. The reference map RM is a part of the environmental map that is being created. Matching is executed so that the position and orientation of the latest scan data SD (b) matches the position and orientation of the previously acquired scan data SD (a). *

By performing such matching, the position and orientation of the moving body 10b on the reference map RM can be known. After the matching is completed, the scan data SD (b) is added to the reference map RM to update the reference map RM. *

The coordinate system of the scan data SD (b) is connected to the coordinate system of the scan data SD (a). This connection is represented in a matrix that defines rotation and translational transformation (rigid transformation) of the two coordinate systems. According to such a conversion matrix, the coordinate value of each point on the scan data SD (b) can be converted into the coordinate value in the coordinate system of the scan data SD (a). *

FIG. 7B shows a reference map RM obtained by adding the next acquired scan data to the reference map RM of FIG. 7A and updating it. In FIG. 7B, a black circle point cloud represents the reference map RM before update, and a white circle dot cloud represents the latest scan data SD (c). FIG. 7B shows the positions a, b, and c of the moving body 10 when the previous scan data, the previous scan data, and the latest scan data are acquired. The whole point group of white circles and the point group of black circles in FIG. 7B constitute an updated reference map RM. *

FIG. 7C shows a reference map RM updated by adding newly acquired scan data SD (d) to the reference map RM of FIG. 7B. In FIG. 7C, the black circle point cloud represents the reference map RM before update, and the white circle dot cloud represents the latest scan data SD (d). In FIG. 7C, in addition to the positions a, b, and c of the moving body 10 at the past estimated position, the position d of the moving body 10 at the position estimated by matching of the latest scan data SD (d) is shown. ing. The whole of the white circle point group and the black circle point group in FIG. 7C constitute an updated reference map RM. *

In this way, since the reference map RM is updated one after another, the number of points in the reference map RM increases every time the external sensor 102 scans. This causes an increase in the amount of calculation when matching the latest scan data with the reference map RM. For example, when one piece of scan data includes a maximum of about 1000 points, when 2000 pieces of scan data are connected to create one reference map RM, the number of points in the reference map RM is about maximum. It reaches 2 million. When the corresponding point is found and the calculation for matching is repeated, if the point group of the reference map RM is too large, the matching may not be completed within the period of Δt that is the scan cycle. *

In the position estimation system according to the present disclosure, the reference map is reset by deleting a part other than the part including the latest scan data from the reference map updated a plurality of times. Moreover, when resetting, it is also possible to update an environmental map based on the reference map updated several times before resetting. The environment map itself prepared in advance by the offline SLAM can be maintained without being updated. *

For example, (i) when the number of times the reference map is updated reaches a predetermined number, (ii) when the data amount of the reference map reaches a predetermined amount, or (iii) since the previous reset. This can be done when the elapsed time reaches a predetermined length. In the case of (i), the “predetermined number” may be 100 times, for example. The “predetermined amount” in the case of (ii) may be 10,000, for example. The “predetermined length” in the case of (iii) can be, for example, 5 minutes. *

To minimize the amount of reference map data after reset, it is only necessary to delete the other scan data while leaving only the latest scan data, that is, the data acquired by the most recent one scan at the time of reset. . When the number of points included in the latest scan data is less than or equal to the predetermined value, in order to improve the matching accuracy after reset, in addition to the latest scan data, include multiple scan data close to the current in the reference map after reset. Also good. *

When creating a reference map from a plurality of scan data, it is useless for matching that the density of points per unit area of a point group increases beyond a predetermined value. For example, when there are a large number of points (measurement points) in a portion corresponding to a rectangular area having a size of 10 × 10 cm ² in the environment, the matching accuracy is sufficiently higher than the rate at which the amount of calculation required for matching increases. Saturation can occur without improvement. In order to suppress such waste, when the density of the point cloud constituting the scan data and / or the reference map exceeds a predetermined density, several points are thinned out from the point cloud, and the density of the point cloud is set to a predetermined density or less. You may perform the process to reduce. The “predetermined density” may be, for example, 1 piece / (10 cm) ² .

<Position Estimation Using Environmental Map> FIG. 8A is a diagram schematically illustrating an example of scan data SD (t) acquired by an external sensor at time t. The scan data SD (t) is displayed in a sensor coordinate system whose position and orientation change together with the moving body 10, and points constituting the scan data SD (t) are described by white circles. *

FIG. 8B is a diagram schematically illustrating a state when matching of the scan data SD (t) with the environment map M is started. When the processor 106 in FIG. 1 acquires the scan data SD (t) from the external sensor 102, the processor 106 performs matching between the scan data SD (t) and the environment map M read from the storage device 104, thereby The position and orientation on the map M can be estimated. When starting such matching, it is necessary to determine initial values of the position and orientation of the moving body 10 at time t (see FIG. 5). The closer the initial value is to the actual position and posture of the moving body 10, the shorter the time required for matching. *

FIG. 8C is a diagram schematically illustrating a state where the matching of the scan data SD (t) with the environment map M is completed. *

In the embodiment of the present disclosure, two types of methods can be employed for determining the initial value. *

In the first method, the amount of change is measured by odometry from the position and orientation estimated by the previous matching. For example, when the moving body 10 is moved by two driving wheels, the moving amount and moving direction of the moving body 10 can be obtained by encoders attached to the respective driving wheels or motors. Since the method using odometry is known, no further detailed explanation is necessary. *

The second method is to predict the current position and posture based on the history of the estimated values of the position and posture of the moving body 10. Hereinafter, this point will be described. *

<Prediction of Initial Value> FIG. 9 is a diagram schematically showing the history of the position and orientation of the moving body 10 obtained in the past by the position estimation system 115 of FIG. 1, and the predicted value of the current position and orientation. . The position and orientation history is stored in the memory 107 inside the position estimation system 115. Part or all of such history may be stored in a storage device outside the position estimation device 105, for example, the storage device 104 in FIG. *

FIG. 9 also shows a UV coordinate system that is a local coordinate system (sensor coordinate system) of the moving body 10. Scan data is expressed in the UV coordinate system. The position of the moving body 10 on the environment map M is the coordinate value (xi, yi) of the origin of the UV coordinate system in the coordinate system of the environment map M. The posture (orientation) of the moving body 10 is the orientation (θi) of the UV coordinate system with respect to the coordinate system of the environment map M. θi is “positive” in the counterclockwise direction.

In the embodiment of the present disclosure, the predicted value of the current position and orientation is calculated from the history of positions and orientations obtained in the past by the position estimation device. *

The position and orientation of the moving body obtained by the previous matching are (x _i−1 , y _i−1 , θ _i−1 ), and the position and orientation of the moving body obtained by the previous matching are further represented by (x _{i -2} , y _i-2 , θ _i-2 ). Further, the predicted value of the current position and orientation of the moving object is (x _i , y _i , θ _i ). At this time, it is assumed that the following assumptions hold.

Assumption 1: The time required for the movement from the position (x _i−1 , y _i−1 ) to the position (x _i , y _i ) is from the position (x _i−2 , y _i−2 ) to the position (x _{i− 1} , y _i-1 ) equal to the time required for movement.

Assumption 2: The moving speed from the position (x _i−1 , y _i−1 ) to the position (x _i , y _i ) is from the position (x _i−2 , y _i−2 ) to the position (x _{i −1} , y _i−1 ) equal to the moving speed during movement.

Under the above process, the following formula 1 is established.

Here, Δθ is θ _i −θ _i−1 .

Regarding the posture (orientation) of the moving body, the following relationship of Formula 2 is established. (Equation 2) θ _i = θ _i−1 + Δθ

When approximation that Δθ is zero is performed, the matrix of the second term on the right side of Equation 2 can be simplified as a unit matrix. *

When the above assumption 1 does not hold, the time required for the movement from the position (x _i−1 , y _i−1 ) to the position (x _i , y _i ) is Δt, and the position (x _i−2 , y _i−2). ) To the position (x _i−1 , y _i−1 ) is Δs. In this case, (x _i−1 −x _i−2 ) and (y _i−1 −y _i−2 ) on the right side of Equation 1 are respectively multiplied by Δt / Δs, and the matrix on the right side of Equation 1 is Correction may be performed by multiplying Δθ by Δt / Δs.

<Operation Flow of Position Estimation System> The operation flow of the position estimation system in the embodiment of the present disclosure will be described with reference to FIGS. 1 and 10 to 13. *

First, referring to FIG. *

In step S <b> 10, the processor 106 of the position estimation system 115 acquires the latest scan data from the external sensor 102. *

In step S12, the processor 106 acquires the current position and orientation values by odometry. At this time, the current position and orientation values may be predicted as described with reference to FIG. *

In step S14, the processor 106 performs initial alignment of the latest scan data with respect to the environment map using the current position and orientation values acquired from odometry as initial values. *

In step S16, the processor 106 performs misregistration correction by the ICP algorithm. *

In step S18, the processor 106 generates a first estimated value of the position and orientation by offline SLAM. *

In step S20, it is determined whether an event has occurred in which the second estimated value of the position and orientation based on the online SLAM is output as the selected estimated value instead of the first estimated value of the position and orientation based on the offline SLAM. To do. In No, it progresses to step S21 and outputs a 1st estimated value as a selected estimated value. Thereafter, the process returns to step S10, and the next scan data is acquired. In the case of Yes, it progresses to step S22. *

An example in the case of being determined as Yes will be described. *

FIG. 11A is a diagram illustrating an example in which the change amount ΔP _t of the first estimated value by the first position estimation process (offline SLAM), that is, P _t −P _t−1 fluctuates. This is because when the first estimated value at the current time t is P _t and the previous first estimated value one time before (for example, 200 milliseconds before) is P _t−1 , the difference is monitored. It is possible to detect an estimated abnormality. For example, when the change amount ΔP _t exceeds the threshold value indicated by the broken line in FIG. 11A, the second estimated value by the second position estimating process (online SLAM) is used instead of the first estimated value by the first position estimating process (offline SLAM). The estimated value can be selected as a more accurate estimated value. In this case, when the amount of change ΔP _t exceeds the threshold value only once, the threshold value is not selected immediately after the second position estimation process (online SLAM) but is continued for a predetermined number of times (for example, three times). You may make it select a 2nd estimated value when exceeding.

FIG. 11B is a diagram illustrating an example in which the difference between the first estimated value obtained by the first position estimating process (offline SLAM) and the measured value obtained by the sensor varies. The sensor is a position and orientation value of the moving body measured by odometry such as a rotary encoder. When the difference between the first estimated value and the measured value exceeds a predetermined range (Range), the second estimated value based on the second position estimating process (online SLAM) is used instead of the first estimated value based on the first position estimating process (offline SLAM). Can be selected as a more accurate estimate. Also in this case, when the difference deviates from the predetermined range only once, the second estimated value by the second position estimation process (online SLAM) is not selected immediately, but the predetermined range is continued for a predetermined number of times (for example, three times). The second estimated value may be selected when deviating from the above. *

FIG. 11C is a diagram illustrating an example in which the reliability of the first estimated value by the first position estimation process (offline SLAM) and the reliability of the second estimated value by the second position estimation process (online SLAM) vary. In the figure, when the reliability of the first estimation indicated by the black circle is lower than the reliability of the second estimation value indicated by the white circle, the first estimation value by the first position estimation process (offline SLAM) The second estimated value obtained by the two-position estimation process (online SLAM) can be selected as a more accurate estimated value. Also in this case, the second estimated value may be selected when the number of times that the reliability of the first estimation is lower than the reliability of the second estimated value exceeds a predetermined number. *

Refer to FIG. 10 again. *

In step S22, the processor 106 performs position and orientation estimation by online SLAM. Specifically, the process proceeds to step S40. The online SLAM flow will be described later. *

Next, with reference to FIG. 12, the displacement correction in step S16 will be described. *

First, in step S32, the processor 106 searches for corresponding points from two sets of point groups. Specifically, the processor 106 selects a point on the environment map corresponding to each point constituting the point group included in the scan data. *

In step S34, the processor 106 performs rigid transformation (coordinate transformation) of rotation and translation of the scan data so as to reduce the distance between corresponding points between the scan data and the environment map. This is to optimize the parameters of the coordinate transformation matrix so as to reduce the distance between corresponding points, that is, the total sum (square sum) of errors of corresponding points. This optimization is performed by iterative calculation. *

In step S36, the processor 106 determines whether or not the result of the iterative calculation has converged. Specifically, the processor 106 determines that it has converged when the amount of reduction in the sum of the errors (corresponding to the squares) of the corresponding points falls below a predetermined value even if the parameters of the coordinate transformation matrix are changed. If not converged, the process returns to step S32, and the processor 106 repeats the processing from the corresponding point search. If it is determined in step S36 that the process has converged, the process proceeds to step S38. *

In step S38, the processor 106 converts the coordinate value of the scan data from the value of the sensor coordinate system to the value of the coordinate system of the environment map using the coordinate conversion matrix. The coordinate values of the scan data obtained in this way can be used for updating the environmental map. *

Next, position and orientation estimation by online SLAM will be described with reference to FIG. *

In step S <b> 40, the processor 106 of the position estimation system 115 acquires the latest scan data from the external sensor 102. *

In step S42, the processor 106 acquires the current position and orientation values by odometry. *

In step S44, the processor 106 performs initial alignment of the latest scan data with respect to the reference map using the current position and orientation values acquired from odometry as initial values. *

In step S <b> 46, the processor 106 performs misalignment correction using an ICP algorithm. *

In step S48, the processor 106 generates an estimated value (second estimated value) of the position and orientation of the moving object obtained as a result of matching with the reference map. This second estimated value is output as a selected estimated value instead of the first estimated value when a “Yes” determination is made in step S20 of FIG. Regardless of the result of the determination in step S20, the estimation of the position and orientation by online SLAM is continuously executed, and the second estimated value is generated. Whether or not to adopt the second estimated value as the selected estimated value depends on the presence or absence of the event described with reference to FIG. *

In step S50, it is determined whether or not the reference map satisfies the update condition. As described above, the update conditions are (i) when the number of times of updating the reference map reaches a predetermined number, (ii) when the data amount of the reference map reaches a predetermined amount, or (iii) since the previous reset. This is a condition such as when the elapsed time of a predetermined length has been reached. In No, it returns to step S40 and acquires the next scan data. In the case of Yes, it progresses to step S52. *

In step S52, the processor 106 deletes a portion other than the portion including the latest scan data from the reference map updated a plurality of times, and resets the reference map. In this way, the number and density of points in the point group constituting the reference map can be reduced. *

Note that the determination performed in step S20 of FIG. 10 is performed at any time or periodically during the online SLAM. For this reason, when there is no need to perform online SLAM, the online SLAM quickly returns to the offline SLAM. *

The position estimation system according to the present disclosure can be applied to various moving bodies that are moved by various driving devices. The position estimation system according to the present disclosure may not be used by being mounted on a moving body including a driving device. For example, it may be placed on a handcart driven by a user and used for map creation. *

<Exemplary Embodiment> Hereinafter, an embodiment of a moving object including the position estimation system according to the present disclosure will be described in more detail. In this embodiment, an automatic guided vehicle is taken as an example of the moving body. In the following description, an abbreviation is used to describe an automatic guided vehicle as “AGV: Automatic Guided Vehicle”. “AGV”
As for the mobile object 10 as well, the reference symbol “10” is attached thereto.

(1) Basic Configuration of System FIG. 14 shows a basic configuration example of an exemplary mobile management system 100 according to the present disclosure. The mobile management system 100 includes at least one AGV 10 and an operation management device 50 that manages the operation of the AGV 10. FIG. 14 also shows a terminal device 20 operated by the user 1. *

The AGV 10 is an automatic guided vehicle capable of “guideless type” traveling that does not require a derivative such as a magnetic tape for traveling. The AGV 10 can perform self-position estimation and transmit the estimation result to the terminal device 20 and the operation management device 50. The AGV 10 can automatically travel in the environment S according to a command from the operation management device 50. *

The operation management device 50 is a computer system that tracks the position of each AGV 10 and manages the running of each AGV 10. The operation management device 50 may be a desktop PC, a notebook PC, and / or a server computer. The operation management device 50 communicates with each AGV 10 via the plurality of access points 2. For example, the operation management device 50 transmits the data of the coordinates of the position to which each AGV 10 should go next to each AGV 10. Each AGV 10 transmits data indicating its position and orientation to the operation management device 50 periodically, for example, every 250 milliseconds. When the AGV 10 arrives at the instructed position, the operation management device 50 transmits data on the coordinates of the position to be further headed. The AGV 10 can also travel in the environment S according to the operation of the user 1 input to the terminal device 20. An example of the terminal device 20 is a tablet computer. *

FIG. 15 shows an example of an environment S in which three

AGVs

10a, 10b, and 10c exist. Assume that all AGVs are traveling in the depth direction in the figure. The

AGVs

10a and 10b are transporting loads placed on the top board. The AGV 10c travels following the front AGV 10b. For convenience of explanation,

reference numerals

10a, 10b, and 10c are attached in FIG. 15, but are described as “AGV10” below. *

In addition to the method of transporting a load placed on the top board, the AGV 10 can also transport the load using a tow cart connected to itself. FIG. 16 shows the AGV 10 and the traction cart 5 before being connected. A caster is provided on each foot of the traction cart 5. The AGV 10 is mechanically connected to the traction cart 5. FIG. 17 shows the AGV 10 and the traction cart 5 connected to each other. When the AGV 10 travels, the tow cart 5 is pulled by the AGV 10. By pulling the tow cart 5, the AGV 10 can transport the load placed on the tow cart 5. *

The connection method between the AGV 10 and the traction cart 5 is arbitrary. Here, an example will be described. A plate 6 is fixed to the top plate of the AGV 10. The pulling cart 5 is provided with a guide 7 having a slit. The AGV 10 approaches the tow truck 5 and inserts the plate 6 into the slit of the guide 7. When the insertion is completed, the AGV 10 passes an electromagnetic lock pin (not shown) through the plate 6 and the guide 7 and applies an electromagnetic lock. Thereby, AGV10 and tow cart 5 are physically connected. *

Refer to FIG. 14 again. Each AGV 10 and the terminal device 20 can be connected, for example, on a one-to-one basis, and can perform communication based on the Bluetooth (registered trademark) standard. Each AGV 10 and the terminal device 20 can perform communication based on Wi-Fi (registered trademark) using one or a plurality of access points 2. The plurality of access points 2 are connected to each other via, for example, the switching hub 3. FIG. 14 shows two

access points

2a and 2b. The AGV 10 is wirelessly connected to the access point 2a. The terminal device 20 is wirelessly connected to the access point 2b. The data transmitted by the AGV 10 is received by the access point 2a, then transferred to the access point 2b via the switching hub 3, and transmitted from the access point 2b to the terminal device 20. The data transmitted by the terminal device 20 is received by the access point 2b, then transferred to the access point 2a via the switching hub 3, and transmitted from the access point 2a to the AGV 10. Thereby, bidirectional communication between the AGV 10 and the terminal device 20 is realized. The plurality of access points 2 are also connected to the operation management device 50 via the switching hub 3. Thereby, bidirectional communication is also realized between the operation management device 50 and each AGV 10. *

(2) Creation of environmental map A map in the environment S is created in advance so that the AGV 10 can run while estimating its own position by offline SLAM. The AGV 10 is equipped with a position estimation device and an LRF, and an environmental map can be created using the output of the LRF. *

The AGV 10 transitions to a data acquisition mode by a user operation. In the data acquisition mode, the AGV 10 starts acquiring sensor data (scan data) using LRF. Subsequent processing is as described above. *

The movement in the environment S for acquiring the sensor data can be realized by the AGV 10 traveling according to the user's operation. For example, the AGV 10 receives a travel command instructing movement in the front, rear, left, and right directions from the user via the terminal device 20 wirelessly. The AGV 10 travels forward / backward / left / right in the environment S according to the travel command and creates a map. When the AGV 10 is connected to a steering device such as a joystick in a wired manner, a map may be created by traveling in the environment S in the front / rear and left / right directions according to a control signal from the steering device. Sensor data may be acquired by a person walking on a measurement carriage equipped with an LRF. *

14 and 15 show a plurality of AGVs 10, the number of AGVs may be one. When there are a plurality of AGVs 10, the user 1 can use the terminal device 20 to select one AGV 10 from among the plurality of registered AGVs and create a map of the environment S. *

(3) Configuration of AGV FIG. 18 is an external view of an exemplary AGV 10 according to the present embodiment. The AGV 10 includes two

drive wheels

11a and 11b, four

casters

11c, 11d, 11e, and 11f, a frame 12, a transport table 13, a travel control device 14, and an LRF 15. The two

drive wheels

11a and 11b are provided on the right side and the left side of the AGV 10, respectively. The four

casters

11c, 11d, 11e, and 11f are arranged at the four corners of the AGV 10. The AGV 10 also has a plurality of motors connected to the two

drive wheels

11a and 11b, but the plurality of motors are not shown in FIG. Also, FIG. 18 shows one drive wheel 11a and two

casters

11c and 11e located on the right side of the AGV 10, and a caster 11f located on the left rear part, but the left drive wheel 11b and the left front part. The caster 11d is not clearly shown because it is hidden behind the frame 12. The four

casters

11c, 11d, 11e, and 11f can freely turn. In the following description, the drive wheels 11a and the drive wheels 11b are also referred to as wheels 11a and wheels 11b, respectively. *

The travel control device 14 is a device that controls the operation of the AGV 10, and mainly includes an integrated circuit including a microcomputer (described later), electronic components, and a board on which they are mounted. The traveling control device 14 performs the above-described data transmission / reception with the terminal device 20 and the preprocessing calculation. *

The LRF 15 is an optical device that measures the distance to a reflection point by, for example, emitting an infrared laser beam 15a and detecting the reflected light of the laser beam 15a. In the present embodiment, the LRF 15 of the AGV 10 emits a pulsed laser beam 15a while changing its direction every 0.25 degrees in a space of 135 degrees left and right (total 270 degrees) with respect to the front of the AGV 10, for example. Then, the reflected light of each laser beam 15a is detected. Thereby, data of the distance to the reflection point in the direction determined by the angle corresponding to the total of 1081 steps every 0.25 degrees can be obtained. In the present embodiment, the scanning of the surrounding space performed by the LRF 15 is substantially parallel to the floor surface and is planar (two-dimensional). However, the LRF 15 may scan in the height direction. *

The AGV 10 can create a map of the environment S based on the position and orientation (orientation) of the AGV 10 and the scan result of the LRF 15. The map may reflect the arrangement of walls, pillars and other structures around the AGV, and objects placed on the floor. The map data is stored in a storage device provided in the AGV 10. *

Hereinafter, the position and orientation of the AGV 10, that is, the pose (x, y, θ) may be simply referred to as “position”. *

As described above, the traveling control device 14 compares the measurement result of the LRF 15 with the map data held by itself, and estimates its current position. The map data may be map data created by another AGV 10. *

FIG. 19A shows a first hardware configuration example of the AGV 10. FIG. 19A also shows a specific configuration of the travel control device 14. *

The AGV 10 includes a travel control device 14, an LRF 15, two

motors

16a and 16b, a drive device 17, and

wheels

11a and 11b. *

The travel control device 14 includes a microcomputer 14a, a memory 14b, a storage device 14c, a communication circuit 14d, and a position estimation device 14e. The microcomputer 14a, the memory 14b, the storage device 14c, the communication circuit 14d, and the position estimation device 14e are connected by a communication bus 14f and can exchange data with each other. The LRF 15 is also connected to the communication bus 14f via a communication interface (not shown), and transmits measurement data as a measurement result to the microcomputer 14a, the position estimation device 14e, and / or the memory 14b. *

The microcomputer 14 a is a processor or a control circuit (computer) that performs an operation for controlling the entire AGV 10 including the travel control device 14. Typically, the microcomputer 14a is a semiconductor integrated circuit. The microcomputer 14a transmits a PWM (Pulse Width Modulation) signal, which is a control signal, to the drive device 17 to control the drive device 17 and adjust the voltage applied to the motor. As a result, each of the

motors

16a and 16b rotates at a desired rotation speed. *

One or more control circuits (for example, a microcomputer) for controlling the driving of the left and

right motors

16a and 16b may be provided independently of the microcomputer 14a. For example, the motor driving device 17 may include two microcomputers that control the driving of the

motors

16a and 16b, respectively. *

The memory 14b is a volatile storage device that stores a computer program executed by the microcomputer 14a. The memory 14b can also be used as a work memory when the microcomputer 14a and the position estimation device 14e perform calculations. *

The storage device 14c is a nonvolatile semiconductor memory device. However, the storage device 14c may be a magnetic recording medium typified by a hard disk or an optical recording medium typified by an optical disk. Furthermore, the storage device 14c may include a head device for writing and / or reading data on any recording medium and a control device for the head device. *

The storage device 14c stores an environment map M of the traveling environment S and data (travel route data) R of one or more travel routes. The environmental map M is created by the AGV 10 operating in the map creation mode and stored in the storage device 14c. The travel route data R is transmitted from the outside after the environment map M is created. In the present embodiment, the environment map M and the travel route data R are stored in the same storage device 14c, but may be stored in different storage devices. *

An example of the travel route data R will be described.

When the terminal device 20 is a tablet computer, the AGV 10 receives travel route data R indicating a travel route from the tablet computer. The travel route data R at this time includes marker data indicating the positions of a plurality of markers. “Marker” indicates the passing position (route point) of the traveling AGV 10. The travel route data R includes at least position information of a start marker indicating a travel start position and an end marker indicating a travel end position. The travel route data R may further include position information of one or more intermediate waypoint markers. When the travel route includes one or more intermediate waypoints, a route from the start marker to the end marker via the travel route point in order is defined as the travel route. The data of each marker may include data on the direction (angle) and traveling speed of the AGV 10 until moving to the next marker, in addition to the coordinate data of the marker. When the AGV 10 temporarily stops at the position of each marker and performs self-position estimation and notification to the terminal device 20, the data of each marker includes acceleration time required for acceleration to reach the travel speed, and / or Further, it may include data of deceleration time required for deceleration from the traveling speed until the vehicle stops at the position of the next marker. *

The operation management device 50 (for example, a PC and / or a server computer) may control the movement of the AGV 10 instead of the terminal device 20. In that case, the operation management device 50 may instruct the AGV 10 to move to the next marker every time the AGV 10 reaches the marker. For example, the AGV 10 receives, from the operation management device 50, coordinate data of a target position to be next, or data of a distance to the target position and an angle to be traveled as travel route data R indicating a travel route. *

The AGV 10 can travel along the stored travel route while estimating its own position using the created map and the sensor data output by the LRF 15 acquired during travel. The details of the operation at this point are as described above. According to the embodiment of the present disclosure, even when a part of the environmental map prepared in advance does not reflect the actual environment, the self-position can be continuously estimated by quickly switching to the online SLAM. *

The communication circuit 14d is a wireless communication circuit that performs wireless communication conforming to, for example, Bluetooth (registered trademark) and / or Wi-Fi (registered trademark) standards. Each standard includes a wireless communication standard using a frequency of 2.4 GHz band. For example, in the mode in which the AGV 10 is run to create a map, the communication circuit 14d performs wireless communication based on the Bluetooth (registered trademark) standard and communicates with the terminal device 20 one-on-one. *

The position estimation device 14e performs map creation processing and self-position estimation processing during traveling. The position estimation device 14e creates a map of the environment S based on the position and orientation of the AGV 10 and the LRF scan result. During traveling, the position estimation device 14e receives sensor data from the LRF 15 and reads the environmental map M stored in the storage device 14c. The local map data (sensor data) created from the scan result of the LRF 15 is matched with a wider range of the environment map M to identify the self-position (x, y, θ) on the environment map M. The position estimation device 14e generates “reliability” data representing the degree to which the local map data matches the environmental map M. Each data of the self position (x, y, θ) and the reliability can be transmitted from the AGV 10 to the terminal device 20 or the operation management device 50. The terminal device 20 or the operation management device 50 can receive each data of its own position (x, y, θ) and reliability and display it on a built-in or connected display device. *

In the present embodiment, the microcomputer 14a and the position estimation device 14e are separate components, but this is an example. It may be a single chip circuit or a semiconductor integrated circuit capable of independently performing the operations of the microcomputer 14a and the position estimation device 14e. FIG. 19A shows a chip circuit 14g including the microcomputer 14a and the position estimation device 14e. Below, the example in which the microcomputer 14a and the position estimation apparatus 14e are provided independently is demonstrated. *

The two

motors

16a and 16b are attached to the two

wheels

11a and 11b, respectively, and rotate each wheel. That is, the two

wheels

11a and 11b are drive wheels, respectively. In the present specification, the motor 16a and the motor 16b are described as being motors that drive the right wheel and the left wheel of the AGV 10, respectively. *

The moving body 10 may further include a rotary encoder that measures the rotational positions or rotational speeds of the

wheels

11a and 11b. The microcomputer 14a may estimate not only the signal received from the position estimation device 14e but also the position and orientation of the moving body 10 using a signal received from the rotary encoder. *

The drive device 17 has

motor drive circuits

17a and 17b for adjusting the voltage applied to each of the two

motors

16a and 16b. Each of

motor drive circuits

17a and 17b includes a so-called inverter circuit. The

motor drive circuits

17a and 17b turn on or off the current flowing through each motor by a PWM signal transmitted from the microcomputer 14a or the microcomputer in the motor drive circuit 17a, thereby adjusting the voltage applied to the motor. *

FIG. 19B shows a second hardware configuration example of the AGV 10. The second hardware configuration example is different from the first hardware configuration example (FIG. 19A) in that it has a laser positioning system 14h and the microcomputer 14a is connected to each component in a one-to-one relationship. To do. *

The laser positioning system 14h includes a position estimation device 14e and an LRF 15. The position estimation device 14e and the LRF 15 are connected by, for example, an Ethernet (registered trademark) cable. Each operation of the position estimation device 14e and the LRF 15 is as described above. The laser positioning system 14h outputs information indicating the pause (x, y, θ) of the AGV 10 to the microcomputer 14a. *

The microcomputer 14a has various general purpose I / O interfaces or general purpose input / output ports (not shown). The microcomputer 14a is directly connected to other components in the travel control device 14 such as the communication circuit 14d and the laser positioning system 14h via the general-purpose input / output port. *

The configuration other than the configuration described above with reference to FIG. 19B is the same as the configuration of FIG. 19A. Therefore, description of a common structure is abbreviate | omitted. *

AGV10 in embodiment of this indication may be provided with safety sensors, such as an obstacle detection sensor and a bumper switch which are not illustrated. *

(4) Configuration Example of Operation Management Device FIG. 20 shows a hardware configuration example of the operation management device 50. The operation management device 50 includes a CPU 51, a memory 52, a position database (position DB) 53, a communication circuit 54, a map database (map DB) 55, and an image processing circuit 56. *

The CPU 51, the memory 52, the position DB 53, the communication circuit 54, the map DB 55, and the image processing circuit 56 are connected by a communication bus 57, and can exchange data with each other. *

The CPU 51 is a signal processing circuit (computer) that controls the operation of the operation management device 50. Typically, the CPU 51 is a semiconductor integrated circuit. *

The memory 52 is a volatile storage device that stores a computer program executed by the CPU 51. The memory 52 can also be used as a work memory when the CPU 51 performs calculations. *

The position DB 53 stores position data indicating each position that can be a destination of each AGV 10. The position data can be represented by coordinates virtually set in the factory by an administrator, for example. The location data is determined by the administrator. *

The communication circuit 54 performs wired communication based on, for example, the Ethernet (registered trademark) standard. The communication circuit 54 is connected to the access point 2 (FIG. 14) by wire, and can communicate with the AGV 10 via the access point 2. The communication circuit 54 receives data to be transmitted to the AGV 10 from the CPU 51 via the bus 57. The communication circuit 54 transmits the data (notification) received from the AGV 10 to the CPU 51 and / or the memory 52 via the bus 57. *

The map DB 55 stores internal map data of a factory or the like where the AGV 10 travels. As long as the map has a one-to-one correspondence with the position of each AGV 10, the format of the data is not limited. For example, the map stored in the map DB 55 may be a map created by CAD. *

The position DB 53 and the map DB 55 may be constructed on a nonvolatile semiconductor memory, or may be constructed on a magnetic recording medium represented by a hard disk or an optical recording medium represented by an optical disk. *

The image processing circuit 56 is a circuit that generates video data to be displayed on the monitor 58. The image processing circuit 56 operates exclusively when the administrator operates the operation management device 50. In the present embodiment, further detailed explanation is omitted. The monitor 59 may be integrated with the operation management device 50. Further, the CPU 51 may perform the processing of the image processing circuit 56. *

In the description of the above-described embodiment, an AGV that travels in a two-dimensional space (floor surface) is taken as an example. However, the present disclosure can also be applied to a moving object that moves in a three-dimensional space, such as a flying object (drone). When a 3D space map is created while a drone is flying, the 2D space can be expanded to a 3D space. *

The comprehensive or specific aspect described above may be realized by a system, a method, an integrated circuit, a computer program, or a recording medium. Alternatively, the present invention may be realized by any combination of a system, an apparatus, a method, an integrated circuit, a computer program, and a recording medium.

The mobile body of the present disclosure can be suitably used for moving and transporting goods such as luggage, parts, and finished products in factories, warehouses, construction sites, logistics, hospitals, and the like.

DESCRIPTION OF SYMBOLS 1 ... User, 2a, 2b ... Access point, 10 ... AGV (moving body), 11a, 11b ... Drive wheel (wheel), 11c, 11d, 11e, 11f ... Caster, 12 ... Frame, 13 ... Conveyance table, 14 ... Travel controller, 14a ... Microcomputer, 14b ... Memory, 14c ... Storage device, 14d ... Communication circuit, 14e ... Position estimation device, 16a, 16b ... motor, 15 ... laser range finder, 17a, 17b ... motor drive circuit, 20 ... terminal device (mobile computer such as tablet computer), 50 ... operation Management device, 51 ... CPU, 52 ... memory, 53 ... position database (position DB), 54 ... communication circuit, 55 ... map database (map D) ), 56 ... image processing circuit, 100 ... mobile management system

Claims

A position estimation system for a moving body that is used by being connected to an external sensor that repeatedly scans the environment and outputs sensor data for each scan, and stores at least one processor and a prepared environment map A first memory; and a second memory for storing a computer program for operating the processor, wherein the at least one processor is configured to: (A) the environment map and the sensor data according to a command of the computer program; A first position estimation process for generating a first estimated value of the position and orientation of the moving body based on the collation result of (2), (B) while generating a reference map of the surroundings using the sensor data, And a second position estimate that generates a second estimated value of the position and orientation of the mobile body based on the comparison result of the sensor data. And (C) selecting one of the first estimated value and the second estimated value, and outputting the selected estimated value as a selected estimated value of the position and orientation of the moving body, Perform a position estimation system.
When executing the second position estimation process, the at least one processor acquires the scan data from the external sensor and creates a reference map from the scan data according to a command of the computer program. When the scan data is newly acquired from the external sensor, the position and orientation of the external sensor on the reference map are estimated by matching the newly acquired latest scan data with the reference map, Updating the reference map by adding the latest scan data to the reference map, deleting a part other than a part including the latest scan data from the reference map updated multiple times, and resetting the reference map. The position estimation system according to claim 1, wherein
3. The processor according to claim 1, wherein the processor calculates a reliability of the first estimated value, and outputs the second estimated value as the selected estimated value when the reliability falls below a threshold value. 4. Position estimation system.
The processor calculates a first reliability of the first estimated value and a second reliability of the second estimated value, and the selected when the first reliability is lower than the second reliability The position estimation system according to claim 1, wherein the second estimated value is output as an estimated value.
The processor outputs the second estimated value as the selected estimated value when a current value of the first estimated value changes from a past value of the first estimated value beyond a predetermined range. Item 5. The position estimation system according to any one of Items 1 to 4.
The processor calculates a difference between the measurement value of the position and / or orientation acquired from the internal sensor and the first estimated value, and when the difference exceeds a threshold value, the second estimated value is selected as the second estimated value. The position estimation system according to claim 1, which outputs an estimated value.
7. The processor according to claim 1, wherein the processor outputs the second estimated value as the selected estimated value when the operation for determining the first estimated value cannot be completed within a predetermined time. 8. Position estimation system.
Acquiring the scan data from the external sensor and creating a reference map from the scan data; when the scan data is newly acquired from the external sensor, matching the newly acquired latest scan data with the reference map To estimate the position and orientation of the external sensor on the reference map, add the latest scan data to the reference map and update the reference map, from the reference map updated multiple times, Deleting a part other than the part including the latest scan data, resetting the reference map, and when performing the reset, an environment map is obtained based on the reference map updated a plurality of times before the reset. The position estimation system according to claim 1, wherein updating is performed.
The position estimation system according to claim 8, wherein the processor resets the reference map when the number of times of updating the reference map reaches a predetermined number.
The position estimation system according to claim 8, wherein the processor resets the reference map when a data amount of the reference map reaches a predetermined amount.
The position estimation system according to claim 8, wherein the processor resets the reference map when an elapsed time from the previous reset reaches a predetermined length.
The processor measures the amount of movement of the external sensor based on the output of the internal sensor, and uses the initial value of the position and orientation of the external sensor used when performing the matching as the amount of movement of the external sensor. The position estimation system according to claim 1, wherein the position estimation system is determined based on the determination.
The processor calculates a predicted value of the current position and orientation of the external sensor based on the history of the position and orientation of the external sensor, and determines the position and orientation of the external sensor used when performing the matching. The position estimation system according to claim 1, wherein the predicted value is used as an initial value.
A moving body comprising the position estimation system according to claim 1, the external sensor, and a driving device for movement.
The mobile body according to claim 14, further comprising an internal sensor.
The computer program used for the position estimation system in any one of Claim 1 to 13.