JP2020024618A - Moving route acquisition method and moving route acquisition apparatus - Google Patents

Abstract

To provide a moving route acquisition method capable of highly precisely calculating a positional relationship on the basis of a quantity of disagreement between local maps acquired at two positions.SOLUTION: A moving route acquisition method includes the steps of: acquiring a local map M2 at each of plural transit points where a moving entity passes; estimating an estimated transit point through map matching between the local map M2 and an environmental map M1; recording a set of plural estimated transit points as a traveling schedule TS; creating the environmental map M1; expanding the range of a first local map M21 or a second local map M22 by acquiring the local map M2 while allowing the moving entity to move in a region near at least one of a first position and a second position; and calculating a relative positional relationship between the first position and the second position on the basis of a quantity of disagreement between the first local map M21 and the second local map M22 whose ranges have been expanded.SELECTED DRAWING: Figure 2

Description

  The present invention relates to a moving route acquisition method for acquiring a moving route of a moving object, and a moving route acquiring device for acquiring a moving route of a moving object.

  2. Description of the Related Art Conventionally, there have been known methods and apparatuses for teaching a moving body such as a robot to move in a predetermined moving environment and reproducing the movement on the moving body. When teaching movement, the moving body acquires (records) a plurality of passing points passed during teaching movement as a taught (moved) movement path. The moving body can reproduce the taught movement by being controlled to pass through a plurality of passing points indicated on the taught movement path.

  When recording the moving route, the moving body uses the SLAM technology to store map information (environmental map) representing the moving environment and map information (local map) acquired by the moving body while teaching movement. Are compared to estimate their own position (self-position), and the estimated self-position is recorded as an estimated passage point corresponding to a passage point on the moving route. While teaching the movement route, the above-mentioned environment map used for estimating the self-position of the moving object is also created by the SLAM technique. When the environment map is created, the mobile unit (1) creates a travel route to the destination when the destination is specified by the user, and moves autonomously to the destination along the travel route. And (2) If the user specifies a predetermined area (fill area) in the environment map, the user can move the entire fill area.

In the SLAM technology for estimating the self-position based on the comparison between the local map and the environment map, for example, when teaching a circular traveling route, the estimated position of the traveling start position and the estimated position of the traveling end position included in the traveling route are determined. May deviate significantly from the actual situation.
In order to solve the above problem, after teaching the movement route, the actual movement start position and the actual movement start position are further determined based on the amount of deviation between the local map acquired at the movement start position and the local map acquired at the movement end position. There is known a technique of calculating a relative positional relationship with a movement end position and correcting each estimated passing point included in a taught moving route based on the positional relationship (for example, Patent Document 1).

JP 2018-22215 A

  In the technique for correcting the estimated passing point described above, in order to appropriately correct the estimated passing point included in the moving route, it is necessary to accurately calculate the positional relationship between the movement start position and the movement end position. In addition, in order to accurately calculate the above positional relationship based on the shift amount between the two local maps acquired at these positions, it is necessary that the degree of overlap between the data included in the two local maps is large. Therefore, conventionally, it has been necessary to make the movement end position when teaching the movement route as close as possible to the vicinity of the movement start position.

  Further, even when the movement end position is brought close to the vicinity of the movement start position, for example, if the amount of data included in the local map acquired at these positions is small, the above positional relationship may not be calculated with high accuracy. there were.

  An object of the present invention is to provide a moving route acquisition method and a moving route acquisition device for calculating a positional relationship between two positions based on a shift amount of a local map acquired at two positions close to each other when teaching the moving route. And to accurately calculate the positional relationship between these positions.

Hereinafter, a plurality of modes will be described as means for solving the problems. These embodiments can be arbitrarily combined as needed.
A moving route acquisition method according to an aspect of the present invention is a method of acquiring a moving route when a moving object is moved from a first position to a second position in a moving environment. The second position is a position near the first position. The moving route acquisition method includes the following steps.
Obtaining a local map at each of a plurality of passing points through which the moving object passes while moving from the first position to the second position. The local map is map information within a predetermined range centered on the location of the moving object in the moving environment.
Estimating the positions of the plurality of passing points in the moving environment as the plurality of estimated passing points by map matching between the local map and the environment map representing the moving environment.
Recording a set of a plurality of estimated passing points as a moving route;
A step of creating an environmental map by arranging each of the plurality of local maps acquired at the plurality of passing points at the corresponding estimated passing point.
A step of enlarging at least one range of the first local map or the second local map by acquiring the local map while the moving body moves in the vicinity area of at least one of the first position and the second position. The first local map is a local map acquired at the first position. The second local map is a local map acquired at the second position.
Calculating a relative positional relationship between the first position and the second position based on a shift amount between the first local map and the second local map in which at least one of the ranges is enlarged;

In the above moving route acquisition method, at least one of two positions (first position and second position) close to each other on the moving route, the local map is acquired while the moving body moves in the vicinity of the selected position. Thus, the range of at least one of the local maps (the first local map and the second local map) acquired at the two positions is enlarged.
Thus, the amount of data included in at least one of the two local maps can be increased, so that the degree of duplication of the data included in the two local maps can be increased. As a result, it is possible to accurately calculate the relative positional relationship between the first position and the second position based on the shift amount between the two local maps in which at least one of the ranges is enlarged.

Enlarging the range of at least one of the first local map and the second local map may include enlarging the range of the second local map.
Thereby, the amount of data included in the local map (second local map) acquired at the second position that reaches later on the taught travel route is increased, and the degree of overlap of the data included in the two local maps is increased. Can be larger.

Enlarging the range of at least one of the first local map and the second local map may include enlarging the range of the first local map.
Thereby, the amount of data included in the local map (first local map) acquired at the first position that arrives first on the taught travel route is increased, and the degree of overlap of the data included in the two local maps is increased. Can be increased.

The local map may be obtained as a set of observation points. The observation point is a point indicating the position of an obstacle existing in a predetermined range.
In this case, the step of enlarging the range of at least one of the first local map and the second local map may include the following steps.
Determining whether the absolute value of the first difference between the number of observation points included in the first local map and the number of observation points included in the second local map is larger than a first threshold value;
And a step of expanding the range of the second local map when the first difference is larger than the first threshold.

This makes it possible to easily determine the degree of duplication of the data included in the two local maps based on the difference in the amount of data included in the two local maps.
Further, when it is determined that the difference between the data amounts included in the two local maps is large and the degree of duplication of the data is small, the data amount of the second local map is increased to increase the data amount of the second local map. The degree of duplication can be improved.

The step of enlarging at least one of the first local map and the second local map may include the following steps.
Determining whether the number of observation points included in the first local map and / or the number of observation points included in the second local map is smaller than a second threshold value;
A step of expanding the range of the first local map and / or the second local map in which the number of observation points is smaller than the second threshold.

Thereby, the degree of duplication of the data included in the two local maps can be easily determined based on the amount of data included in the two local maps.
When it is determined that the amount of data included in the two local maps is small and the degree of duplication of the data is small, the data amount of the local map having the small data amount is increased to increase the data amount of the data included in the two local maps. The degree of duplication can be improved.

The step of enlarging at least one of the first local map and the second local map may include the following steps.
Determining whether there is a gap larger than a predetermined size between adjacent observation points included in the first local map and the second local map;
Extracting a group of observation points existing between two gaps as a cluster;
Determining whether a second difference between the number of clusters included in the first local map and the number of clusters included in the second local map is larger than a third threshold value;
ス テ ッ プ expanding the range of the second local map when the second difference is larger than the third threshold value;

Thus, the similarity between the two local maps can be easily determined based on the number of clusters included in the two local maps.
Further, since the difference between the numbers of clusters included in the two local maps is large and the similarity between the two local maps is low, when it is determined that the degree of overlap of the data included in the two local maps is small, the second local map is determined. By increasing the data amount of the map, the degree of redundancy of the data included in the two local maps can be improved.

The step of enlarging the range of at least one of the first local map and the second local map may further include the following steps.
Calculating a third difference between the number of observation points included in one cluster of the first local map and the number of observation points included in one corresponding cluster of the second local map.
A step of determining whether or not the third difference is larger than a fourth threshold value;
ス テ ッ プ expanding the range of the second local map when the third difference is larger than the fourth threshold value;

Thereby, the degree of duplication of the data in each cluster included in the two local maps can be easily determined.
Further, when it is determined that the difference between the amount of data in a cluster included in one local map and the amount of data in a cluster included in the other local map is large and the degree of duplication of data in each cluster is low, By increasing the data amount of the second local map, the degree of redundancy of data included in the two local maps can be improved.

The step of enlarging the range of at least one of the first local map and the second local map may further include the following steps.
Determining whether or not the number of observation points included in the first local map is smaller than a second threshold value;
* A step of enlarging the range of the first local map when the number of observation points included in the first local map is smaller than the second threshold.

  Thereby, when it is determined that the amount of data included in the first local map is small and the degree of similarity between the two local maps and / or the degree of duplication of data in each cluster included in the two local maps is small. By increasing the data amount of the first local map, the degree of redundancy of data included in the two local maps can be improved.

A moving route acquisition device according to another aspect of the present invention is a device that acquires a moving route when a moving object is moved from a first position to a second position in a moving environment. And a control unit.
The control unit is map information within a predetermined range centering on a position of the moving body in a moving environment at each of a plurality of passing points through which the moving body passes while moving from the first position to the second position. Obtain a local map, estimate the positions of multiple passing points in the moving environment as multiple estimated passing points by map matching between the local map and the environment map representing the moving environment, and use a set of multiple estimated passing points as the moving path. Is stored in the storage unit.
Further, the control unit creates an environment map by arranging each of the plurality of local maps acquired at the plurality of passing points at the corresponding estimated passing point, and stores the created environment map in the storage unit.
Further, the control unit obtains the local map while the moving body moves in the vicinity of at least one of the first position and the second position, so that the first local map or the second position obtained at the first position is obtained. And expanding the at least one range of the second local map acquired at step (1), based on the amount of deviation between the first local map and the second local map in which at least one of the ranges is expanded. Is calculated.

In the moving route acquisition device described above, the control unit determines that at least one of the two positions (the first position and the second position) close to each other on the moving route, the moving body moves locally in the vicinity of the selected position. By acquiring the map, the range of at least one of the local maps (the first local map and the second local map) acquired at the two positions is enlarged.
Thus, the amount of data included in at least one of the two local maps can be increased, so that the degree of duplication of the data included in the two local maps can be increased. As a result, it is possible to accurately calculate the relative positional relationship between the first position and the second position based on the shift amount between the two local maps in which at least one of the ranges is enlarged.

The control unit may enlarge the range of the second local map.
Thereby, the amount of data included in the local map (second local map) acquired at the second position that reaches later on the taught travel route is increased, and the degree of overlap of the data included in the two local maps is increased. Can be larger.

The control unit may enlarge the range of the first local map.
Thereby, the amount of data included in the local map (first local map) acquired at the first position that arrives first on the taught travel route is increased, and the degree of overlap of the data included in the two local maps is increased. Can be increased.

The control unit may acquire the local map as a set of observation points indicating the locations of obstacles existing in a predetermined range.
In this case, the control unit determines whether the absolute value of the first difference between the number of observation points included in the first local map and the number of observation points included in the second local map is larger than a first threshold. And
When the first difference is larger than the first threshold, the range of the second local map may be expanded.

This makes it possible to easily determine the degree of duplication of the data included in the two local maps based on the difference in the amount of data included in the two local maps.
Further, when it is determined that the difference between the data amounts included in the two local maps is large and the degree of duplication of the data is small, the data amount of the second local map is increased to increase the data amount of the second local map. The degree of duplication can be improved.

The control unit determines whether the number of observation points included in the first local map and / or the number of observation points included in the second local map is smaller than a second threshold,
The range of the first local map in which the number of observation points is smaller than the second threshold and / or the range of the second local map may be enlarged.

Thereby, the degree of duplication of the data included in the two local maps can be easily determined based on the amount of data included in the two local maps.
When it is determined that the amount of data included in the two local maps is small and the degree of duplication of the data is small, the data amount of the local map having the small data amount is increased to increase the data amount of the data included in the two local maps. The degree of duplication can be improved.

The control unit determines whether there is a gap larger than a predetermined size between adjacent observation points included in the first local map and the second local map,
The observation point group existing between the two gaps is extracted as a cluster,
It is determined whether a second difference between the number of clusters included in the first local map and the number of clusters included in the second local map is larger than a third threshold value,
When the second difference is larger than the third threshold, the range of the second local map may be expanded.

Thus, the similarity between the two local maps can be easily determined based on the number of clusters included in the two local maps.
Further, since the difference between the numbers of clusters included in the two local maps is large and the similarity between the two local maps is low, when it is determined that the degree of overlap of the data included in the two local maps is small, the second local map is determined. By increasing the data amount of the map, the degree of redundancy of the data included in the two local maps can be improved.

The control unit calculates a third difference between the number of observation points included in one cluster of the first local map and the number of observation points included in one corresponding cluster of the second local map,
It may be determined whether or not the third difference is larger than the fourth threshold, and if the third difference is larger than the fourth threshold, the range of the second local map may be expanded.

Thereby, the degree of duplication of the data in each cluster included in the two local maps can be easily determined.
Further, when it is determined that the difference between the amount of data in a cluster included in one local map and the amount of data in a cluster included in the other local map is large and the degree of duplication of data in each cluster is low, By increasing the data amount of the second local map, the degree of redundancy of data included in the two local maps can be improved.

The control unit determines whether or not the number of observation points included in the first local map is smaller than a second threshold. When the number of observation points included in the first local map is smaller than a second threshold, The range of the first local map may be enlarged.
Thereby, when it is determined that the amount of data included in the first local map is small and the degree of similarity between the two local maps and / or the degree of duplication of data in each cluster included in the two local maps is small. By increasing the data amount of the first local map, the degree of redundancy of data included in the two local maps can be improved.

  A moving route obtaining method and a moving route obtaining method for obtaining a moving route when a moving body is moved from a first position to a second position in a moving environment include a first local map at a first position and a second local map at a second position. When calculating the positional relationship between the first position and the second position based on the amount of deviation from the local map, the amount of data included in at least one of these local maps is increased as necessary, and The degree of duplication of data included in the map can be improved. As a result, the above positional relationship can be accurately calculated.

FIG. 1 is a diagram showing a configuration of a moving object to which an embodiment of the present invention is applied. The figure which shows the structure of a control system. 9 is a flowchart showing an outline of the operation of the moving object. 9 is a flowchart showing the operation of the moving body in the manual mode. 9 is a flowchart showing an operation of reinforcing a local map. 9 is a flowchart illustrating an operation of generating an environment map and a travel schedule. The figure which shows the operation example of a moving body. The figure which shows an example of a 1st local map. The figure which shows typically the reinforcement processing of a 1st local map. The figure which shows an example of the travel schedule before correction. The figure which shows an example of the environment map before correction. The figure which shows an example of a 2nd local map. The figure which shows typically the reinforcement processing of a 2nd local map. The figure which shows an example of the travel schedule after amendment. The figure which shows an example of the environment map after amendment. The figure which shows the state which arranged the 1st local map and the 2nd local map after reinforcement on the same environmental map. The figure which shows the state which arrange | positioned the 1st local map and 2nd local map without reinforcement on the same environmental map. 9 is a flowchart illustrating a method for determining a degree of coincidence between a first local map and a second local map according to the second embodiment. The figure which shows an example when the determination of the matching degree using a cluster is useful.

1. 1. First Embodiment (1) Outline of Moving Route Obtaining Apparatus Hereinafter, a moving path obtaining apparatus 100 according to a first embodiment will be described.
The moving route acquisition device 100 according to the first embodiment is mounted on a moving body 1 that autonomously travels on a predetermined moving route. The moving route acquisition device 100 is a device that acquires coordinates of a point at which the moving body 1 has passed while the user travels the moving body 1, and acquires and records a set of the coordinates as a taught moving route.
The moving body 1 runs autonomously in the moving environment ME so as to pass through each coordinate point recorded on the taught moving path. Thus, the mobile unit 1 can autonomously travel while faithfully reproducing the taught travel route.

(2) Configuration of Moving Body Hereinafter, the configuration of the moving body 1 on which the movement route acquisition device 100 is mounted will be described with reference to FIGS. 1 and 2. FIG. 1 is a diagram showing a configuration of a moving body to which an embodiment of the present invention is applied. FIG. 2 is a diagram illustrating a configuration of the control system.
The moving body 1 includes a main body 11, a moving unit 12, an obstacle information acquiring unit 13, and a control system 14.
The main body 11 is, for example, a housing that forms the main body of the moving body 1. In the present embodiment, the “self position” described later is defined as the position (coordinates) of the center of the main body 11 on the environment map M1 representing the moving environment ME. In addition, the term “self” refers to the main body 11 of the moving object 1.

The moving unit 12 moves the moving body 1 (main body 11) in the moving environment ME. The moving unit 12 of this embodiment is a traveling device of a differential two-wheel type, and has a pair of motors 121a and 121b and a pair of wheels 123a and 123b.
The pair of motors 121a and 121b are electric motors provided at the bottom of the main body 11, such as a servo motor and a brushless motor. The pair of motors 121a and 121b independently rotate their output rotary shafts at an arbitrary number of rotations and torque based on a command from a control system 14 (described later).

  As shown in FIG. 2, encoders 125a and 125b are provided on output rotation shafts of the motors 121a and 121b, respectively. The encoders 125a and 125b output pulse signals based on the rotation amounts of the output rotation shafts of the motors 121a and 121b. As such encoders 125a and 125b, for example, incremental encoders can be used.

A part of each of the pair of wheels 123a and 123b is in contact with the floor (moving surface) of the moving environment ME, and is connected to the output rotation shaft of the corresponding motor 121a or 121b. Thereby, the wheels 123a and 123b are independently rotated by the motors 121a and 121b, respectively, to move the main body 11.
Since the independent rotation is possible as described above, it is possible to change the rotation speed of the wheels 123a and 123b to change the posture of the main body 11. On the other hand, if the rotation speeds of the pair of wheels 123a and 123b are the same, the main body 11 can go straight.

The obstacle information acquisition unit 13 acquires information on obstacles and / or objects existing within a predetermined range centered on the location of the moving body 1 (main body 11) in the moving environment ME.
In the present embodiment, the obstacle information acquisition unit 13 has a first laser range sensor 131 and a second laser range sensor 133.
The first laser range sensor 131 and the second laser range sensor 133 transmit, for example, a laser beam pulsed by a laser oscillator to a structure in the moving environment ME (for example, a column, a shelf, or a wall disposed in the moving environment ME). A laser range finder (LRF: Laser Range) that obtains information on the obstacle or the object by irradiating the obstacle or the object radially and receiving the reflected light reflected from the obstacle with a laser receiver. Finder).

The first laser range sensor 131 is provided in front of the main body 11, and generates a laser beam radially in front of the moving body 1 (forward direction), so that the main body around the first laser range sensor 131 is generated. Information about an obstacle or an object included in a semicircle with a radius of about 5 m in front of No. 11 (that is, a viewing angle of 180 degrees) is obtained.
On the other hand, the second laser range sensor 133 is provided at the rear of the main body 11, and generates a laser beam radially in the left-right direction behind (in the backward direction) the moving body 1 so that the second laser range sensor 133 is centered on the second laser range sensor 133. Information about obstacles and objects contained within a semicircle with a radius of about 5 m behind the main body 11 (that is, a viewing angle of 180 degrees) is obtained.
Note that the detectable distance of the laser range sensor is not limited to the above value, and can be changed as appropriate according to the use of the moving body 1 and the like.

  In addition, as the sensor for detecting the above-mentioned obstacle or object, a sensor that can measure the distance between the sensor (main body 11) and the surrounding obstacle or object other than the laser range finder is used. it can. For example, a TOF (Time Of Flight) camera can be used. Further, a system that can operate a sensor that measures a one-dimensional or two-dimensional distance as a device that measures a two-dimensional or three-dimensional distance can be used.

  In the following, an example using two-dimensional (XY coordinate system) data relating to an object will be described. However, the following description applies to three-dimensional (XYZ coordinate system) data. Applicable to

  The control system 14 includes a CPU (Central Processing Unit), a hard disk device, a ROM (Read Only Memory), a RAM (Random Access Memory), a storage device such as an HDD and / or an SSD, and a computer including various interfaces for performing signal conversion. It is a system configured by the system. The control system 14 controls each part of the moving body 1.

  As shown in FIG. 1, the moving body 1 may further include an auxiliary wheel 15. The auxiliary wheel unit 15 has two auxiliary wheels 15a and 15b. The two auxiliary wheels 15a and 15b are attached so that each can rotate independently. The provision of the auxiliary wheel portion 15 allows the moving body 1 to move stably and smoothly.

(3) Configuration of Control System The configuration of the control system 14 provided in the moving object 1 will be described with reference to FIG.
The control system 14 includes a traveling route acquisition device 100, an autonomous traveling control device 300, and a traveling control device 500.
The travel route acquisition device 100 is a device that acquires a travel route on which the moving body 1 autonomously reproduces and travels. The moving route acquisition device 100 includes a storage unit 101 and a control unit 103.
The storage unit 101 is a part of a storage area of the control system 14. The storage unit 101 stores an environment map M1, a local map M2, and a travel schedule TS (an example of an acquired travel route).

The environment map M1 is map information indicating a moving environment ME in which the moving object 1 moves.
The local map M2 (first local map M21, second local map M22) is map information within a predetermined range centered on the location of the main body 11 (mobile body 1) in the mobile environment ME.
The traveling schedule TS stores a plurality of coordinate points in the moving environment ME through which the moving body 1 operated by the user has passed. That is, the travel schedule TS stores the travel route R1 taught by the user as a set of a plurality of coordinate points.

  The control unit 103 executes a process related to obtaining the movement route R1. Specific processing contents executed by the control unit 103 will be described later in detail.

  The autonomous traveling control device 300 calculates a traveling command for causing the moving body 1 to travel based on the traveling schedule TS stored in the storage unit 101, and outputs the traveling command to the traveling control device 500.

The travel control device 500 is connected to the motors 121a and 121b. The travel control device 500 calculates a control amount of the motors 121a and 121b based on a travel command input from the autonomous travel control device 300, and outputs drive power based on the control amount to each of the motors 121a and 121b.
When the travel control device 500 controls the motors 121a and 121b based on the travel command, the moving body 1 can autonomously travel while faithfully reproducing the travel schedule TS. In addition, the mobile unit 1 can autonomously travel so as to fill an arbitrary area designated by the user on the environment map M1 according to the travel command. Further, the mobile unit 1 plans a movement route to the destination specified by the user on the environment map M1 according to the travel command, and can autonomously travel to the destination along the planned movement route. Hereinafter, the operation mode of the moving body 1 that controls the motors 121a and 121b based on the traveling command is referred to as “autonomous mode”.

In addition, the travel control device 500 is a controller or a computer system capable of communicating with the mobile unit 1 wirelessly or by wire, or a user using an operation device such as an operation handle (not shown) provided on the mobile unit 1. Operation can be accepted.
The travel control device 500 calculates a control amount of the motors 121a and 121b based on a user operation received from the controller or the like, and outputs drive power based on the control amount to each of the motors 121a and 121b.
By controlling the motors 121a and 121b based on a user operation received from a controller or the like, the moving body 1 can travel according to the user operation. Hereinafter, the operation mode of the moving body 1 that controls the motors 121a and 121b based on the operation of the user is referred to as "manual mode".

In the travel control device 500, the rotation amount (rotation speed) of the motors 121a and 121b per unit time corresponds to the rotation amount corresponding to the travel speed indicated by the travel command or the user's operation amount in the controller or the like. The control amount of the motors 121a and 121b is calculated so that the rotation amount is obtained (feedback control).
The rotation speed of each of the motors 121a and 121b can be calculated from the number of pulses per unit time output from the encoders 125a and 125b.

In the present embodiment, the functions of the traveling route acquisition device 100, the autonomous traveling control device 300, and the traveling control device 500 are realized as a program executed in a computer system constituting the control system 14. The program is stored in a storage device of the control system 14.
In another embodiment, the traveling route acquisition device 100, the autonomous traveling control device 300, and / or the traveling control device 500 may be configured by individual computer systems. In this case, the control system 14 has a plurality of computer systems.
Further, a part of the above three devices may be configured by a computer system, and the functions of the other devices may be realized by a program executable by the computer system.
Further, some or all of the functions of the above-described device may be realized by a custom IC such as an SoC.

(4) Operation of Moving Object (4-1) Outline of Operation Hereinafter, an operation of the moving object 1 according to the first embodiment having the above configuration will be described. First, an outline of the operation of the moving body 1 will be described with reference to FIG. FIG. 3 is a flowchart showing an outline of the operation of the moving object.
When the moving body 1 starts operating, it is determined in step S1 whether or not the user has selected an operation mode. Specifically, for example, when an operation mode selection instruction signal is input from the operation device, it is determined that the user has selected the operation mode.

When the operation mode is selected by the user (“Yes” in step S1), the control system 14 determines in step S2 whether the selected operation mode is the manual mode or the autonomous mode.
If the selected operation mode is the manual mode (“manual mode” in step S2), the control system 14 executes the manual mode (step S3). Specifically, the control system 14 receives an operation by the user of the operation device, and controls the motors 121a and 121b according to the received operation of the user.

  In the manual mode, while the motors 121a and 121b are controlled in accordance with the operation of the user, the traveling route acquisition device 100 of the control system 14 generates (creates) the traveling schedule TS and uses the SLAM technology to generate the environment map M1. Is generated (created), and both are stored in the storage unit 101. As a result, an environment map M1 representing the moving environment ME in which the moving body 1 moves and a traveling schedule TS representing the moving route R1 of the moving body 1 taught by the user's operation can be generated. The details of the operation in the manual mode in step S3 will be described later in detail.

On the other hand, when it is determined that the selected operation mode is the autonomous mode (“autonomous mode” in step S2), the autonomous traveling control device 300 of the control system 14 executes the autonomous mode in step S4. Specifically, the autonomous mode is executed as follows.
First, the autonomous driving control device 300 performs the map matching between the environment map M1 generated in step S3 and the local map M2 acquired while moving the moving object 1 in the autonomous mode, so that the moving object moving in the autonomous mode. Estimate one's own position and / or posture.
After that, the autonomous traveling control device 300 calculates a control command for moving to the next target point indicated in the traveling schedule TS from the estimated self-position and / or posture, and outputs the control command to the traveling control device 500. Thereby, the motors 121a and 121b are controlled based on the control command output from the autonomous traveling control device 300.
By repeatedly performing the above-described processing from the movement start position to the movement end position of the travel schedule TS, the moving body 1 can autonomously move according to the travel schedule TS.

Thereafter, for example, the user instructs the stop of the manual mode or the autonomous mode using the operation device, or the mobile unit 1 passes through all the passing points indicated in the traveling schedule TS in the autonomous mode and moves to the movement end position. Steps S1 to S4 are continuously executed until it is determined that the time has arrived (“No” in step S5).
On the other hand, when the manual mode or the autonomous mode is stopped, or when the moving body 1 reaches the movement end position indicated in the traveling schedule TS in the autonomous mode (“Yes” in step S5), the moving body 1 performs its operation. To end.

(4-2) Operation in Manual Mode Next, the operation in the manual mode (moving route acquisition operation) executed in step S3 will be described with reference to FIGS. 4 and 5. FIG. 4 is a flowchart showing the operation of the moving body in the manual mode. FIG. 5 is a flowchart showing a local map reinforcing operation.
In the following, an example will be described in which the moving body 1 is taught and moved along an annular movement path R1 from the movement start position (first position P1) to the movement end position (second position P2) of the movement environment ME. I do. That is, in the following example, correction of an environment map M1 and a travel schedule TS, which will be described later, is performed.
When the estimated position of the movement start position and the estimated position of the movement end position of the movement route R1 do not largely differ from the actual position, such as when the movement route R1 is not circular, the environment map M1 and the travel schedule TS May not necessarily be executed.

When the user moves the moving body 1 to the first position P1 and a user issues a manual mode execution command via an operation device or the like, the manual mode is started.
At this time, the user may instruct, via an operation device or the like, whether or not to execute a reinforcing operation of the first local map M21 and / or the second local map M22, which will be described later, or the control system 14 may automatically perform the operation. May be determined to perform the reinforcement operation.

  When the manual mode is started and the local map reinforcement operation is performed, the control unit 103 of the movement route acquisition apparatus 100 acquires the local map M2 at the first position P1 in step S301, and sets the acquired local map M2 as the first local map M21. . The local map M2 can be obtained, for example, as follows.

First, the control unit 103 changes the angles of the laser irradiation units (not shown) of the first laser range sensor 131 and the second laser range sensor 133 with respect to the obstacle information acquisition unit 13 within a predetermined angle range ( (Scanning) while instructing to irradiate a laser beam. Thereby, information on the object within the viewing angle of the obstacle information acquisition unit 13 can be acquired.
While irradiating the laser beam while changing the angle of the laser irradiating unit, the control unit 103 controls the information on the angle of the laser irradiating unit and the time difference between the timing of irradiating the laser beam and the timing of receiving the reflected light of the laser beam. And information about the data for each predetermined data acquisition cycle.
Thereafter, a distance between the obstacle information acquisition unit 13 and an object existing around the moving body 1 is calculated based on the information on the time difference. Further, the direction in which the object is viewed from the main body 11 can be calculated from the angle of the laser irradiation unit.

Further, the control unit 103 converts the calculated relative distance of the object viewed from the main body 11 and the angle of the laser irradiation unit into coordinate values on a coordinate plane representing the moving environment ME.
For example, when the distance from the moving body 1 to the object is calculated as R, and the angle of the laser irradiation unit is α (counterclockwise is a positive angle) with respect to the straight traveling direction of the moving body 1 (FIG. 1). The relative position of the object with respect to the moving body 1 is expressed by (R * cosα, R) in a coordinate system in which the origin is the center of the obstacle information acquisition unit 13 and the moving direction of the moving body 1 is the x direction (horizontal direction). * Sin α).

  Note that the above coordinate conversion need not be performed. In this case, the local map M2 includes data that associates the distance R from the moving object 1 (the main body 11) to the object existing in the moving environment ME and the direction α in which the object exists with respect to the straight traveling direction of the moving object 1. Become.

  In this manner, the local map M2 is obtained as a set of coordinate points representing the position of an object existing within a predetermined range around the moving body 1 with the position of the moving body 1 as the origin. You. The acquired local map M2 is stored in the storage unit 101.

After acquiring the first local map M21, the control unit 103 determines in step S302 that the acquired first local map M21 is suitable for calculating the relative positional relationship between the first position P1 and the second position P2. It is determined whether or not it is.
Here, the case where the first local map M21 becomes inappropriate for calculating the relative positional relationship between the first position P1 and the second position P2 will be briefly described.
As described above, the coordinate points included in the local map M2 are obtained by changing the angle of the laser irradiation unit of the obstacle information obtaining unit 13 within a predetermined angle range. In this case, depending on the arrangement and / or shape of the obstacle (wall or the like) existing around the moving body 1, the laser light emitted from the obstacle information acquisition unit 13 may not be reflected by the obstacle to be detected.
For example, when only the reflected light of another obstacle existing in front of the obstacle to be detected (closer to the moving body 1) is received by the obstacle information acquisition unit 13, the obstacle to be detected is determined by the obstacle information acquisition unit. For example, when the position is not within the measurement range of No. 13, the coordinate value of the obstacle to be detected cannot be obtained, and the local map M2 has a "missing" coordinate point.

  When the number of coordinate points included in the first local map M21 decreases due to the lack of coordinate points as described above, the coordinate points corresponding to the coordinate points included in the second local map M22 acquired at the second position P2 are obtained. The number of coordinate points on the first local map M21 decreases. As a result, it becomes impossible to accurately calculate the relative positional relationship between the first position P1 and the second position P2 by matching the two local maps.

Therefore, in the present embodiment, the number of coordinate points (an example of an observation point) included in the first local map M21 is determined in order to accurately calculate the relative positional relationship between the first position P1 and the second position P2. By determining whether or not the number is sufficient, it is determined whether or not the first local map M21 is appropriate.
More specifically, whether or not the first local map M21 is appropriate is determined based on whether or not the number of coordinate points included in the first local map M21 is smaller than the second threshold.

  When it is determined that the number of coordinate points included in the first local map M21 is equal to or greater than the second threshold ("No" in step S302), it is determined that the first local map M21 is appropriate, and the manual mode is selected. The execution process proceeds to step S304. That is, generation of the environment map M1 and the travel schedule TS is started.

On the other hand, when it is determined that the number of coordinate points included in the first local map M21 is smaller than the second threshold ("Yes" in step S302), it is determined that the first local map M21 is inappropriate. In step S303, a process of reinforcing the first local map M21 is executed.
In the present embodiment, the process of reinforcing the local map M2 (the first local map M21 and the second local map M22) is executed according to the flowchart shown in FIG.

First, in step S41, the moving body 1 is moved in a region near the first position P1.
It is preferable that the vicinity area be at least a range determined by an installation error when the moving body 1 is arranged at the first position P1 in order to move the moving body 1.
For example, the moving body 1 is moved by 1 m or more from the current installation position of the moving body 1, and the posture of the moving body 1 is ± 10 degrees or more from the posture angle when the moving body 1 is installed at the first position P1. The moving range when the range is changed can be set as the neighboring region.
In addition, for example, a semicircle having a radius of 1 m or more around the current installation position (when the viewing angle of the obstacle information acquisition unit 13 is set to 180 degrees) may be set as the nearby area. Thus, an object in all directions (360 degrees) around the first position P1 can be detected by the obstacle information acquisition unit 13 (the first laser range sensor 131 and the second laser range sensor 133).
The neighborhood area is not limited to the above example, and can be any area as long as the number of coordinate points included in the first local map M21 can be equal to or larger than the second threshold value.

However, the movement of the moving body 1 in step S41 is a regular movement (for example, either increasing or decreasing the change in the distance from the current installation position and the posture of the moving body 1), rather than a random movement. Only one of them). This makes it easier to combine the local maps M2 acquired during the movement, so that the first local map M21 can be easily reinforced.
Further, it is preferable to move the moving body 1 at a low speed. Thereby, the accuracy of the self-position estimation at each position during the movement can be improved, so that the first local map M21 can be appropriately reinforced in a state in which the state around the first position P1 is more accurately reflected.

  The movement of the moving body 1 in step S41 may be executed by a user operation, or may be executed autonomously under the control of the control unit 103.

During the movement of the moving body 1, in step S42, the control unit 103 acquires the local map M2 at the current self-position at a predetermined cycle.
Then, in step S43, the self-position at the time when the local map M2 is obtained is estimated.

The self-position estimation performed in step S43 is performed, for example, by calculating a moving distance based on the amount of rotation of the wheels 123a and 123b when moving from the self-position estimated last time to the current self-position. In consideration of the posture (moving direction) when the robot has moved from the current position to the current position, the distance can be calculated by adding the moving distance to the previous position. The posture change of the moving body 1 can be calculated based on the difference between the rotation amounts of the two wheels 123a and 123b (dead reckoning).
In addition, for example, the moving distance is calculated based on the amount of deviation between the local map M2 acquired at the previous self-position and the local map M2 acquired at the current self-position, and from the previous self-position to the current self-position. The calculation may be performed by adding the moving distance to the previous self-position in consideration of a change in the posture when moving. In this case, the posture change of the moving body 1 can be calculated based on, for example, the rotation angle of the one local map when matching one local map with the other local map.

  Further, the self-position and the posture of the mobile unit 1 may be estimated by map matching between the local map M2 and the environment map. In this case, the environment map may be the entire environment map M1 or an environment map for reinforcing the local map M2, that is, map information of a predetermined range including the first position P1 of the environment map M1 (of the environment map M1). Part).

  Further, the previously acquired local map M2 is moved by the moving distance calculated based on the rotation amounts of the wheels 123a and 123b, and the self-position and the self-position are determined by map matching between the moved local map M2 and the currently acquired local map M2. The posture of the moving body 1 may be estimated.

Steps S42 and S43 described above are repeatedly executed at a predetermined cycle until the movement of the mobile unit 1 in step S41 ends (“No” in step S44).
As a result, a plurality of local maps M2 acquired at different self-positions can be acquired from the start to the end of the movement of the moving body 1.

After acquiring the plurality of local maps M2, in step S45, the control unit 103 combines the first local map M21 with the plurality of local maps M2 acquired at different self-positions around the first position P1.
When the local map M2 is acquired at a position different from the first position P1, coordinate points that cannot be acquired at the first position P1 (not included in the first local map M21) can be acquired. Therefore, by combining the first local map M21 and a plurality of local maps M2 acquired at different self-positions, the number of coordinate points included in the first local map M21 is increased, and the range of the first local map M21 is expanded. it can.

After obtaining the first local map M21 having a sufficient number of coordinate points as described above, in step S304, the environment map M1 and the travel schedule TS are generated.
The operation of generating the environment map M1 and the travel schedule TS in step S304 will be described later in detail.

After generating the environment map M1 and the traveling schedule TS, in step S305, the control unit 103 converts the local map M2 at the second position P2 into the second local map M2 in the same manner as the method of acquiring the first local map M21. It is acquired as the map M22 and stored in the storage unit 101.
The second local map M22 obtained as described above is a set of coordinate points representing the location of an object existing within a predetermined range around the moving body 1 (the second location P2) with the second location P2 as the origin. It is.

After acquiring the second local map M22, in step S306, the control unit 103 determines whether the number of coordinate points included in the second local map M22 is smaller than a second threshold.
When the number of coordinate points in the second local map M22 is smaller than the second threshold (“Yes” in step S306), the control unit 103 performs the above steps S41 to S45 in step S307 and The movement is performed on the second local map M22 by moving in the vicinity area of the position P2 to increase the number of coordinate points included in the second local map M22. Thereby, the range of the second local map M22 can be expanded.

  On the other hand, whether the number of coordinate points of the second local map M22 acquired in step S305 is equal to or greater than the second threshold (“No” in step S306), or the number of coordinate points in the second local map M22 in step S307 Is increased, the control unit 103 determines in step S308 whether the first local map M21 and the second local map M22 sufficiently match.

Here, the reason for determining the degree of coincidence between the first local map M21 and the second local map M22 will be described.
Even if the number of coordinate points of the first local map M21 and the second local map M22 is equal to or larger than the second threshold, if there is a large difference between the shape of the first local map M21 and the shape of the second local map M22. The probability that the coordinate points included in the second local map M22 correspond to the coordinate points included in the first local map M21 is reduced. As a result, it becomes impossible to accurately calculate the relative positional relationship between the first position P1 and the second position P2 by matching the two local maps.
Therefore, the degree of coincidence between the first local map M21 and the second local map M22 is determined, and when the degree of coincidence is low, the second local map M22 is further reinforced to further enhance the first local map M21 and the second local map M22. The degree of coincidence of the map M22 can be improved. As a result, the relative positional relationship between the first position P1 and the second position P2 can be calculated more accurately.
Further, it has been found that there is a correlation between the degree of matching between the shapes of the two local maps M2 acquired at close positions and the degree of matching of the number of coordinate points included in each of the two local maps M2. .

Therefore, in the present embodiment, in step S308, the control unit 103 determines the difference between the number of coordinate points included in the first local map M21 and the number of coordinate points included in the second local map M22 (referred to as a first difference). By determining whether or not the absolute value of the first local map M21 is greater than the first threshold value, it is determined whether or not the degree of coincidence between the first local map M21 and the second local map M22 is high.
When the absolute value of the first difference is equal to or smaller than the first threshold ("No" in step S308), it is determined that the degree of coincidence between the first local map M21 and the second local map M22 is sufficiently high, and the execution process in the manual mode is performed. Proceeds to step S310. That is, correction of the environment map M1 and the travel schedule TS is started.

On the other hand, when the absolute value of the first difference is larger than the first threshold (“Yes” in step S308), it is determined that the degree of coincidence between the first local map M21 and the second local map M22 is low, and the control unit 103 In step S309, the above-described steps S41 to S45 are further performed on the second local map M22 by moving the moving body 1 in the area near the second position P2 to further reinforce the second local map M22. .
If the process of increasing the number of coordinate points in the second local map M22 has been performed in step S307, the moving range of the moving object 1 in step S309 is different from the moving range in step S307. You may. For example, the moving range of the moving body 1 in step S309 may be extended more than the moving range in step S307.
Thereby, the range of the second local map M22 can be further expanded by further including the coordinate points not included in the second local map M22 reinforced in step S307 in the second local map M22 in step S309. As a result, the degree of coincidence between the first local map M21 and the second local map M22 reinforced in step S309 can be further improved.

The above steps S301 to S309 are executed to obtain the first local map M21, the second local map M22, the environment map M1, and the travel schedule TS, and, if necessary, the first local map M21 and / or the second local map M21. After reinforcing the map M22, in step S310, the control unit 103 corrects the environment map M1 and the traveling schedule TS acquired in step S304.
Specifically, the environment map M1 and the travel schedule TS are corrected as described below.

First, the control unit 103 calculates an estimated distortion amount that is a deviation amount of the travel schedule TS from the actual travel route R1.
Specifically, a first positional relationship representing a relative positional relationship between the first position P1 and the second position P2, and an estimated passing point (described later) corresponding to the first position P1 in the travel schedule TS (first estimated) The estimated distortion is determined based on the relative positional relationship between a second passing relationship (referred to as a passing point) and an estimated passing point (referred to as a second estimated passing point) corresponding to the second position P2. Calculate the amount.

The first positional relationship can be calculated by calculating the amount of movement of one of the first local map M21 and the second local map M22 with respect to the other using a map matching technique.
Specifically, for example, a histogram indicating the frequency of the coordinate points indicating the presence of the object in the first local map M21 and a frequency indicating the frequency of the coordinate points indicating the presence of the object in the second local map M22 are shown. The amount of movement of one of the histograms when the histogram and the histogram match can be calculated as the amount of deviation between the first local map M21 and the second local map M22 (histogram matching method).

In addition, the movement amount when any one of the local maps is moved so as to minimize the distance between each coordinate point included in the first local map M21 and the corresponding coordinate point included in the second local map M22. Can be calculated as the amount of deviation between the first local map M21 and the second local map M22 (Iterative Closest Point method, ICP method).
Furthermore, by combining the above-described histogram matching method and the ICP method, the movement amount of any one of the local maps that matches the first local map M21 with the second local map M22 can be calculated as the above-mentioned deviation amount. .

  Next, the control unit 103 determines, for example, a first estimated pass point that is an estimated pass point recorded first in the travel schedule TS and a second estimated pass point that is an estimated pass point recorded last. The difference is calculated as a second positional relationship.

Thereafter, the control unit 103 calculates an estimated distortion amount based on the relative positional relationship between the first positional relationship and the second positional relationship. In the present embodiment, the difference between the first positional relationship and the second positional relationship is calculated as the estimated distortion amount.
As another embodiment of the method for calculating the estimated distortion amount, for example, the difference between the first positional relationship and the second positional relationship may be multiplied by an appropriately changeable coefficient to obtain the estimated distortion amount. Thus, the user can arbitrarily designate the degree of correction of the travel schedule TS and the environment map M1.

After calculating the estimated distortion amount, the control unit 103 corrects the travel schedule TS by distributing the calculated estimated distortion amount to the entire travel schedule TS using, for example, Graph SLAM.
In particular, by dispersing the distortion of the traveling schedule TS that has increased at the end of the teaching movement over the entire traveling schedule TS, the traveling schedule TS can be made closer to the actual traveling route R1.

  After correcting the travel schedule TS, for example, the control unit 103 arranges the local map M2 acquired at the corresponding pass point at each of the corrected estimated pass points included in the corrected travel schedule TS, and The environment map M1 is corrected by rotating the local map M2 placed at the point by an angle corresponding to the (corrected) estimated posture at each of the estimated passing points and combining the local maps M2 on these estimated passing points. I do.

As described above, the first position P1 is calculated by calculating the relative positional relationship between the first position P1 and the second position P2 based on the amount of deviation between the first local map M21 and the second local map M22. The original first position P1 (movement start position) and the second position P2 (movement end position) of the travel schedule TS can be determined without moving the moving body 1 to the same position as the above.
As a result, the travel schedule TS that needs to be executed after the movement start position (first position P1) and the movement end position (second position) of the travel schedule TS are determined is, for example, corrected to the first position P1. The teaching movement requiring skill, such as making the second position P2 the same, can be executed without the user or the moving body 1.
Further, by reinforcing the first local map M21 and the second local map M22 as necessary, the relative positional relationship between the first position P1 and the second position P2 can be calculated more accurately. As a result, the travel schedule TS and the environment map M1 can be corrected more appropriately.

(4-3) Generation Operation of Environment Map and Travel Schedule Hereinafter, the generation operation of the environment map M1 and the travel schedule TS, which is executed in the manual mode described above, will be described with reference to FIG. FIG. 6 is a flowchart showing an operation for generating an environment map and a travel schedule.
First, in step S51, the moving body 1 is moved from the movement start position (the first position P1) to the movement end position (the second position P2) of the movement route R1 by the user's operation using the operation device described above. Moving the moving body 1 based on the operation of the user is referred to as “teaching movement”.

During the teaching movement, in step S52, the control unit 103 acquires the local map M2 at predetermined time intervals (for example, at every control cycle of the moving object 1).
Thereby, the local map M2 is acquired at every predetermined time during which the moving body 1 is moving on the moving route R1, that is, at each of a plurality of passing points on the moving route R1 where the moving body 1 has passed.
Note that the first local map M21 described above may be used as the local map M2 at the first position P1. Further, the above-described second local map M22 may be used as the local map M2 at the second position P2.

  After acquiring the local map M2, in step S53, the control unit 103 estimates the current own position and / or posture of the moving object 1 in the moving environment ME as an estimated passage point and / or estimated posture, respectively. The estimation of the estimated passing point and / or the estimated attitude is specifically performed as follows. In addition, even when the moving body 1 moves autonomously in the autonomous mode, the autonomous traveling control device 300 executes the following self-position estimation.

  The control unit 103 first calculates the moving distance from the previous self-position to the current self-position based on the rotation amounts of the wheels 123a and 123b acquired from the encoders 125a and 125b. Next, in consideration of the moving direction (posture) of the moving body 1, the present self position is calculated (estimated) by adding the above moving distance to the self position previously estimated. The local map M2 acquired in step S52 is arranged at a position on the environment map M1 corresponding to the calculated self-position.

Next, at this position, the control unit 103 rotates the local map M2 by the posture (angle) calculated based on the rotation amounts of the wheels 123a and 123b, and performs map matching between the environment map M1 and the local map M2. Do.
At this time, in consideration of the position and / or posture estimation error estimated based on the rotation amounts of the wheels 123a and 123b, the width is set to the arrangement position and the rotation angle when the local map M2 is arranged on the environment map M1. To have. Specifically, the local map M2 is arranged at a plurality of positions on the environment map M1 within a predetermined range centered on the position and / or the posture estimated based on the rotation amounts of the wheels 123a and 123b, and In each of the positions, the local map M2 is rotated within a predetermined rotation angle range.

  After that, the control unit 103 estimates, from the plurality of arrangement positions and the plurality of rotation angles, the arrangement position and the rotation angle at which the local map M2 most closely matches the environment map M1 as the own position and the posture, respectively. The estimation of the self-position and / or posture by such map matching can be performed by, for example, “likelihood calculation”.

After estimating the current estimated passing point and / or estimated posture, the control unit 103 generates an environment map M1 in step S54 and stores the generated environment map M1 in the storage unit 101.
Specifically, the local map M2 acquired in step S52 is arranged at a position (coordinate) corresponding to the current estimated passing point on the environment map M1 that has been generated so far. Thereafter, the local map M2 is rotated at an angle corresponding to the estimated attitude at the position on the environment map M1.
As a result, a new environment map M1 can be generated by combining the local map M2 acquired in step S52 with the environment map M1 generated so far.
Note that the arrangement and rotation of the local map M2 on the environment map M1 are based on a matrix calculation for translating and rotating coordinates on a coordinate plane (XY coordinate system) defining the environment map M1, respectively. This can be realized by executing the process on M2.

After generating the new environment map M1, the control unit 103 generates a travel schedule TS in step S55.
Specifically, the current estimated pass point and / or estimated attitude estimated in step S53 is additionally recorded in the travel schedule TS in which the estimated estimated pass points and / or estimated attitudes are recorded. , A new travel schedule TS is generated.
When the current estimated passing point and / or the estimated attitude is stored in the travel schedule TS, the time at which the local map M2 used for the estimation of the current estimated passing point and / or the estimated attitude is obtained, or the current passing point is obtained. , The speed of the moving body 1 (the amount of operation by the operation device) may be stored. By storing these pieces of information in the travel schedule TS, for example, the travel control device 500 can easily control the motors 121a and 121b according to the travel schedule TS.

After generating the new travel schedule TS, the control unit 103 determines in step S56 whether to end the generation of the environment map M1 and the travel schedule TS.
For example, when the user has not instructed the end of the teaching movement using the operation device or the like ("No" in step S56), the control unit 103 determines that the generation of the environment map M1 and the travel schedule TS is to be continued, and Steps S51 to S55 are executed again.
On the other hand, for example, when the user instructs the end of the teaching movement using the operation device or the like (“Yes” in step S56), the control unit 103 ends the generation of the environment map M1 and the travel schedule TS.

(5) Operation Example Hereinafter, an operation example of the moving object 1 by executing the above processing will be described.
In the following, an example will be described of an operation in the case where the moving body 1 is taught the annular moving route R1 shown in the same figure in the moving environment ME as shown in FIG. That is, the first position P1 that is the movement start position of the movement route R1 and the second position P2 that is the movement end position are relatively close. FIG. 7 is a diagram illustrating an operation example of the moving object.
When the teaching of the movement route R1 is started, the above-described step S301 is executed, and the first local map M21 is obtained at the movement start position (first position P1). In this operation example, it is assumed that the first local map M21 as indicated by the solid line in FIG. 8 has been obtained. FIG. 8 is a diagram illustrating an example of the first local map.
After acquiring the first local map M21, step S302 is executed, and the number of coordinate points included in the first local map M21 is counted. As a result of the counting, if the number of coordinate points of the first local map M21 is smaller than the second threshold, steps S41 to S45 are executed, and the process of increasing the number of coordinate points included in the first local map M21 (reinforcement) Process) is executed.

In the reinforcement processing of the first local map M21, as shown in FIG. 9, for example, while the moving body 1 moves in the vicinity area, the local map M2 is acquired at a predetermined cycle, and the acquired local map M21 and the acquired local map M21 are acquired. By combining the map M2, a first local map M21 after reinforcement is generated.
FIG. 9 is a diagram schematically illustrating a process of reinforcing the first local map.

In the example illustrated in FIG. 9, the neighboring area is an area surrounded by the orbital route R2 having the first position P1 as a start point and an end point. That is, the moving body 1 moves on the orbit route R2.
In the first local map M21 shown in FIG. 9, the local map shown by the thick dotted line is the original first local map M21. On the other hand, a local map M2 indicated by a thick solid line is a local map M2 added by reinforcement.
By the above-described reinforcement processing, as shown in FIG. 9, a new local map (coordinate point) is added to the original first local map M21, and the first local map M21 having an enlarged range is generated.

After acquiring the first local map M21, steps S51 to S56 are executed, and while the moving body 1 moves along the movement route R1 from the movement start position (first position P1) to the movement end position (second position P2), An environment map M1 and a travel schedule TS are generated.
The travel schedule TS generated by moving the movement route R1 to the movement end position (the second position P2) is generated as a set of estimated passing points as shown in FIG. FIG. 10 is a diagram illustrating an example of a travel schedule before correction.
As shown in FIG. 10, the traveling schedule TS generated at this stage moves away from the actual traveling route R1 as the estimated passing point approaches the movement end position (second position P2). This is because an estimation error occurs each time the estimated estimating point is estimated by the movement route acquisition device 100, and the error is accumulated as the teaching movement progresses.

  As a result of the travel schedule TS deviating from the actual travel route R1 as described above, the environment map M1 generated by arranging the local map M2 corresponding to each estimated pass point of the travel schedule TS is also shown in FIG. Is different from the actual moving environment ME. FIG. 11 is a diagram illustrating an example of the environment map before correction. As shown in FIG. 11, the start and end of the environment map M1 immediately after generation are not connected.

After generating the environment map M1 and the travel schedule TS, step S305 is executed, and a second local map M22 as shown in FIG. 12 is obtained at the second position P2. FIG. 12 is an example of the second local map.
After that, the number of coordinate points included in the second local map M22 is counted. As a result of the counting, if the number of coordinate points of the second local map M22 is smaller than the second threshold, steps S41 to S45 are executed, and the reinforcement processing of the second local map M22 is executed.
Further, in step S308, the absolute value of the first difference between the number of coordinate points included in the first local map M21 and the number of coordinate points included in the second local map M22 is calculated. When the absolute value of the first difference is larger than the first threshold, steps S41 to S45 are further executed, and further reinforcement processing of the second local map M22 is executed.

The reinforcement processing of the second local map M22 includes, for example, acquiring the local map M2 at a predetermined cycle while moving the nearby area to the moving body 1, and combining the first local map M21 with the acquired local map M2. Thus, a second local map M22 after reinforcement as shown in FIG. 13 is generated. FIG. 13 is a diagram schematically illustrating a process of reinforcing the second local map.
In the example illustrated in FIG. 13, the neighboring area is the orbital route R3 having the second position P2 as a start point and an end point. That is, the moving body 1 moves on the orbit route R3.
As a result of the above-described reinforcement processing, as shown in FIG. 13, a new local map is added to the original second local map M22, and a second local map M22 having an enlarged range is generated.

Thereafter, step S310 is executed, and the environment map M1 and the travel schedule TS are corrected as described above based on the (reinforced) first local map M21 and the (reinforced) second local map M22. You.
As a result of the correction of the travel schedule TS, as shown in FIG. 14, each estimated passing point of the travel schedule TS is arranged on the movement route R1. In particular, the second estimated position of the travel schedule TS substantially coincides with the second position P2 of the traveling route R1. FIG. 14 is a diagram illustrating an example of the travel schedule after the correction.
Further, by arranging the local maps M2 corresponding to the respective estimated passing points of the corrected travel schedule TS, it is possible to generate an environment map M1 accurately reflecting the moving environment ME as shown in FIG. FIG. 15 is a diagram illustrating an example of the corrected environment map.

(6) Conclusion As described above, in the moving route acquisition device 100 according to the present embodiment, the control unit 103 controls the moving route R1 in at least one of the first position P1 and the second position P2 close to each other. The range of the first local map M21 and the range of the second local map M22 are enlarged by acquiring the local map M2 while moving the area near the position to the moving object 1.

Thereby, the number of coordinate points (data amount) included in the first local map M21 and the second local map M22 can be increased, so that the degree of overlap of data included in the two local maps can be increased.
For example, the degree of coincidence (degree of overlap) between the reinforced first local map M21 and the second local map M22 shown in FIG. 16 obtained in the above operation example is the two local maps without reinforcement shown in FIG. It can be seen that it is larger than the degree of coincidence (degree of overlap) of the maps.
In FIGS. 16 and 17, the first local map M21 is indicated by a thick dotted line, and the second local map M22 is indicated by a thick solid line.
FIG. 16 is a diagram showing a state where the first local map and the second local map after reinforcement are arranged on the same environment map. FIG. 17 is a diagram showing a state where the first local map and the second local map without reinforcement are arranged on the same environment map.

  By reinforcing the first local map M21 and the second local map M22, the number of coordinate points included in the first local map M21 and the second local map M22 can be increased. As a result, the degree of overlap between the two local maps can be improved, and the relative positional relationship between the first position P1 and the second position P2 can be accurately determined based on the amount of displacement between the two local maps whose ranges have been enlarged. Can be calculated.

Further, whether to expand the range of the first local map M21 and / or the second local map M22 is determined based on the number of coordinate points included in these local maps. Thereby, the degree of duplication of the data included in the two local maps can be easily determined based on the number of coordinate points included in the two local maps.
Further, the second local map M22 is increased based on the degree of overlap between the first local map M21 and the second local map M22, that is, based on the absolute value of the first difference in the number of coordinate points included in these local maps. It has been decided whether or not to make it. This makes it possible to easily determine the degree of duplication of the data included in the two local maps based on the difference in the number of coordinate points included in the two local maps.

2. 2. Second Embodiment (1) Outline The method of determining the degree of coincidence between the first local map M21 and the second local map M22 in step S308 is not limited to the method based on the magnitude of the absolute value of the first difference.
In the second embodiment, the degree of coincidence between the first local map M21 and the second local map M22 is calculated as a difference between the number of clusters included in the first local map M21 and the number of clusters included in the second local map M22. Judgment based on
Further, based on the difference between the number of coordinate points included in one cluster of the first local map M21 and the number of coordinate points included in a corresponding one cluster of the second local map M22, the first local map The degree of coincidence between M21 and the second local map M22 is determined.
In the present embodiment, the “cluster” refers to a group of coordinate points (an example of an observation point group) between two gaps having a predetermined size or more that exist in the local map M2.
The “gap” of the local map M2 refers to a “space” existing between adjacent coordinate points included in the local map M2.

In the second embodiment, the moving route acquisition device 100 differs from the first embodiment only in the method of determining the degree of coincidence between the first local map M21 and the second local map M22. The configuration and functions of the control system 14 are the same as those in the first embodiment.
Therefore, description of the configurations and functions of the mobile unit 1 and the control system 14 other than the method of determining the degree of coincidence between the first local map M21 and the second local map M22 is omitted here.

(2) Determination Operation Using Cluster Hereinafter, a method of determining the degree of coincidence between the first local map M21 and the second local map M22 using the cluster in the second embodiment will be described with reference to FIG. FIG. 18 is a flowchart illustrating a method for determining the degree of coincidence between the first local map and the second local map according to the second embodiment.
When the processing for determining the degree of coincidence between the first local map M21 and the second local map M22 is started, the control unit 103 first determines in step S61 the clusters included in the first local map M21 and the second local map M22. Execute the extraction process (clustering process).
Specifically, the clustering process is executed as follows.

First, the control unit 103 determines whether a gap larger than a predetermined size exists between coordinate points included in the first local map M21 and the second local map M22.
Specifically, for example, the coordinate points included in the first local map M21 and the second local map M22 are sequentially scanned, and the coordinate value of the current coordinate point is compared with the coordinate value of the immediately preceding coordinate point adjacent thereto. If the coordinate values of these adjacent coordinate points are farther than a predetermined threshold value, it is determined that there is a gap between these adjacent coordinate points.

Next, a group of coordinate points existing between the two gaps is extracted as one cluster.
Specifically, for example, when scanning the coordinate points in the above, an identification number for identifying a cluster can be associated with each coordinate point. More specifically, in the above scan, when a gap was detected between the current coordinate point and the immediately preceding coordinate point, the current coordinate point was associated with the immediately preceding coordinate point. An identification number whose identification number is changed (for example, the identification number is incremented by 1) is associated, and thereafter, the same identification number is associated with each coordinate point until the next gap is detected.
In this way, it is possible to identify which cluster each coordinate point belongs to.

After performing the above-described clustering process on each of the first local map M21 and the second local map M22, the number of clusters included in each local map is counted.
After that, in step S62, the control unit 103 sets the absolute value of the difference between the number of clusters included in the first local map M21 and the number of clusters included in the second local map (referred to as a second difference) to the third It is determined whether it is larger than the threshold value.
When the absolute value of the second difference is larger than the third threshold ("Yes" in step S62), the process of determining the degree of coincidence proceeds to step S64. That is, the control unit 103 determines that the degree of coincidence between the first local map M21 and the second local map M22 is low.

On the other hand, when the absolute value of the second difference is equal to or smaller than the third threshold (“No” in step S62), in step S63, the number of coordinate points included in one specific cluster of the first local map M21 is further determined. , The degree of coincidence with the number of coordinate points included in the corresponding cluster of the second local map M22 is determined.
In the present embodiment, the “corresponding cluster” means that the identification number of the corresponding cluster matches the identification number of the cluster in the first local map M21 to be compared. .

The degree of coincidence between the number of coordinate points included in one specific cluster of the first local map M21 and the number of coordinate points included in the corresponding cluster of the second local map M22 is included in these two local maps. When the number of clusters is close, it indicates the degree of coincidence between the shapes of these two local maps.
Therefore, by determining the degree of coincidence between the number of coordinate points included in one specific cluster of the first local map M21 and the number of coordinate points included in the corresponding cluster of the second local map M22, these two are determined. By matching the two local maps, it can be determined whether or not the relative positional relationship between the first position P1 and the second position P2 can be accurately calculated.

The determination in step S63 is specifically performed as follows.
First, the control unit 103 determines the difference between the number of coordinate points included in one cluster of the first local map M21 and the number of coordinate points included in the corresponding cluster of the second local map M22 (the third difference and the third difference). Call) is calculated.
In the present embodiment, the absolute value of the third difference is calculated for all the clusters included in the first local map M21 and the second local map M22.

Next, the control unit 103 determines whether or not the sum of the absolute values of the third differences is larger than a fourth threshold value. When the sum of the absolute values of the third differences is larger than the fourth threshold value (“Yes” in step S63), the number of coordinate points included in one specific cluster of the first local map M21 and the second local map M22 It is determined that the degree of coincidence with the number of coordinate points included in the corresponding cluster is low.
In this case, the determination process proceeds to step S64, and determines that the degree of coincidence between the first local map M21 and the second local map M22 is low.

On the other hand, when the sum of the third differences is equal to or smaller than the fourth threshold (“No” in step S63), the number of coordinate points included in one specific cluster of the first local map M21 and the number of coordinate points of the second local map M22 are determined. It is determined that the degree of coincidence with the number of coordinate points included in the corresponding cluster is high.
In this case, the determination process proceeds to step S65, and determines that the degree of coincidence between the first local map M21 and the second local map M22 is high.

In the determination of the degree of coincidence between the number of coordinate points included in one specific cluster of the first local map M21 and the number of coordinate points included in the corresponding cluster of the second local map M22, the third difference is determined. The sum of the absolute values may be the sum of the absolute values of the third differences of all clusters, or the sum of the absolute values of the third differences of some clusters.
When the sum of the absolute values of the third differences of some of the clusters is used, for example, even if only the absolute values of the third differences are large (that is, the difference in the number of coordinate points included in the cluster is large), Good. In this case, it may be determined whether the absolute value of one third difference is larger than the fourth threshold.

According to the above-described determination method using clusters, the degree of coincidence (similarity) between the first local map M21 and the second local map M22 can be easily determined based on the number of clusters included in these two local maps.
That is, when there is a large difference in the number of clusters included in the two local maps, it can be easily determined that the degree of coincidence between the first local map M21 and the second local map M22 is low.

In addition, by the above-described determination method using clusters, it is possible to easily determine the degree of overlap of the coordinate points in each cluster included in the two local maps. 2 The degree of coincidence of the local map M22 can be easily determined.
That is, even if the number of clusters included in the two local maps is close, if there is a large difference in the number of coordinate points included in each cluster, it is determined that the degree of coincidence between the first local map M21 and the second local map M22 is low. Easy to determine.
When the number of clusters included in the two local maps is close and the difference in the number of coordinate points included in each cluster is small, it is determined that the degree of coincidence between the first local map M21 and the second local map M22 is high. Easy to determine.

For example, as shown in FIG. 19, the first position P1 and the second position P2 are close to an obstacle (wall), and the first position P1 is determined by the nearby obstacle. This is particularly useful when an obstacle (a portion circled by a dotted line in FIG. 19) that could be detected from the moving body 1 cannot be detected from the moving body 1 at the second position P2.
FIG. 19 is a diagram illustrating an example of a case where the determination of the matching degree using the cluster is useful.

That is, in the example shown in FIG. 19, the number of clusters (3 in the example of FIG. 19) of the second local map M22 is smaller than the number of clusters (4 in the example of FIG. 19) of the first local map M21. It is determined that the degree of coincidence between the map M21 and the second local map M22 is low.
In this case, the range of the second local map M22 is expanded, and the reinforcement corresponding to the cluster 2 (FIG. 19) of the first local map M21 is included in the second local map M22.

3. Other Embodiments A plurality of embodiments of the present invention have been described above, but the present invention is not limited to the above embodiments, and various changes can be made without departing from the spirit of the invention. In particular, a plurality of embodiments and modifications described in this specification can be arbitrarily combined as needed.
The order of the processes in the flowcharts shown in FIGS. 3 to 6 and FIG. 18 can be changed as appropriate. Also, some processes can be omitted.

  (A) For example, steps S302 and S303 shown in FIG. 4 may be omitted. That is, for the first local map M21, the determination regarding the number of coordinate points included in the local map and the reinforcement for the local map need not be performed. In other words, reinforcement may be performed only for the second local map M22.

  (B) Steps S308 and S309 shown in FIG. 4 may be omitted. That is, the second local map M22 is not reinforced based on the degree of coincidence (degree of overlap) between the first local map M21 and the second local map M22, and the number of coordinate points included in the second local map M22 is smaller or larger. May be performed only on the second local map M22 based on.

  (C) Steps S306 and S307 shown in FIG. 4 may be further omitted. That is, the second local map M22 is not reinforced based on the number of coordinate points included in the second local map M22, and the degree of coincidence (degree of overlap) between the first local map M21 and the second local map M22 is high or low. May be performed only on the second local map M22 based on.

  (D) Steps S302, S306, S308, and S309 shown in FIG. 4 may be omitted. That is, without performing any determination, the first local map M21 and / or the second local map M22 may be expanded based on the premise that the range of the first local map M21 and / or the second local map M22 is required. May be implemented.

(E) When acquiring the local map M2 at one position, the moving body 1 is stopped at the position for a while to acquire a plurality of local maps M2, and the coordinates of the coordinate points included in the plurality of local maps M2 are acquired. By averaging the values, a new local map M2 at the position may be calculated.
The process of averaging the coordinate values to obtain a new local map M2 may be executed when the first local map M21 and / or the second local map M22 are reinforced.
Thereby, the local map M2 with few noise components can be acquired.

(F) The above-described reinforcement processing of the local map M2 may be executed at a position other than the movement start position and / or the movement end position of the movement route R1.
For example, the moving body 1 may automatically detect a passing point that has passed through the vicinity a plurality of times on the moving route R1 and reinforce the local map M2 at the passing point.

(G) Reinforcement of the local map M2 may be executed at an arbitrary position designated by the user.
For example, when it is known in advance that the moving body 1 will make a round around the pillar existing in the moving environment ME while moving along the moving route R1, the timing at which the moving body 1 reaches the vicinity of the pillar and the pillar The user can instruct using the operating device of the moving body 1 to execute the reinforcement of the local map M2 at the timing when the tour of the vehicle has been completed.
In addition, for example, when moving the moving body 1 in and out of a dead-lane path existing in the moving environment ME, and when passing the neighborhood of the dead-lane path, the moving body 1 tries to enter the dead-lane path. The execution of the reinforcement of the local map M2 can be instructed at (one end of the route) and the timing of going out of the route (the other end of the route).

  INDUSTRIAL APPLICABILITY The present invention can be widely applied to an apparatus and a method for teaching a moving path when a moving object is moved in a predetermined moving environment.

DESCRIPTION OF SYMBOLS 1 Moving body 11 Main body 12 Moving parts 121a, 121b Motors 123a, 123b Wheels 125a, 125b Encoder 13 Obstacle information acquisition part 131 First laser range sensor 133 Second laser range sensor 14 Control system 100 Moving path acquisition device 101 Storage part 103 Control unit 300 Autonomous traveling control device 500 Traveling control device 15 Auxiliary wheels 15a, 15b Auxiliary wheels M1 Environmental map M2 Local map M21 First local map M22 Second local map ME Moving environment P1 First position P2 Second position R1 Moving path R2, R3 circuit route TS travel schedule

Claims (16)

A moving route acquisition method for acquiring a moving route when a moving object is moved from a first position in a moving environment to a second position near the first position,
At each of a plurality of passing points through which the moving object passes while moving from the first position to the second position, map information within a predetermined range centered on the position of the moving object in the moving environment is used. Obtaining a local map;
Estimating the positions of the plurality of passing points in the moving environment as a plurality of estimated passing points by map matching between the local map and an environment map representing the moving environment,
Recording a set of the plurality of estimated pass points as a travel route,
Creating the environment map by arranging each of the plurality of local maps acquired at the plurality of passing points at the corresponding estimated passing point,
The first local map or the second position acquired at the first position by acquiring the local map while the moving body moves in at least one of the vicinity regions of the first position or the second position. Enlarging at least one range of the second local map obtained at
Calculating a relative positional relationship between the first position and the second position based on a shift amount between the first local map and the second local map in which at least one range is enlarged;
And a moving route acquisition method.   The moving route acquisition method according to claim 1, wherein enlarging at least one range of the first local map or the second local map includes enlarging a range of the second local map.   3. The moving route acquisition method according to claim 1, wherein expanding the range of at least one of the first local map and the second local map includes expanding the range of the first local map. 4. The local map is obtained as a set of observation points representing the location of obstacles existing in the predetermined range,
The step of expanding at least one of the first local map and the second local map includes:
Determining whether an absolute value of a first difference between the number of observation points included in the first local map and the number of observation points included in the second local map is larger than a first threshold value; ,
Expanding the range of the second local map when the absolute value of the first difference is larger than the first threshold value;
The moving route acquisition method according to claim 1, comprising: The local map is obtained as a set of observation points representing the location of obstacles existing in the predetermined range,
The step of expanding at least one of the first local map and the second local map includes:
Determining whether the number of observation points included in the first local map and / or the number of observation points included in the second local map is smaller than a second threshold value;
Expanding the range of the first local map and / or the second local map in which the number of observation points is smaller than the second threshold value;
The moving route acquisition method according to claim 1, further comprising: The local map is obtained as a set of observation points representing the location of obstacles existing in the predetermined range,
The step of expanding at least one of the first local map and the second local map includes:
Determining whether there is a gap larger than a predetermined size between the adjacent observation points included in the first local map and the second local map,
Extracting a group of observation points existing between the two gaps as a cluster;
Determining whether an absolute value of a second difference between the number of clusters included in the first local map and the number of clusters included in the second local map is larger than a third threshold value,
Expanding the range of the second local map when the absolute value of the second difference is larger than the third threshold value;
The moving route acquisition method according to claim 2, comprising: The step of expanding at least one of the first local map and the second local map includes:
Calculating a third difference between the number of observation points included in one cluster of the first local map and the number of observation points included in one corresponding cluster of the second local map; When,
Determining whether the absolute value of the third difference is greater than a fourth threshold;
Expanding the range of the second local map when the absolute value of the third difference is greater than the fourth threshold value;
The moving route acquisition method according to claim 6, further comprising: The step of expanding at least one of the first local map and the second local map includes:
Determining whether the number of the observation points included in the first local map is smaller than a second threshold,
Expanding the range of the first local map when the number of observation points included in the first local map is smaller than the second threshold value;
The moving route acquisition method according to claim 6, further comprising: A moving route acquisition device for acquiring a moving route when a moving body is moved from a first position in a moving environment to a second position near the first position,
A storage unit,
At each of a plurality of passing points through which the moving object passes while moving from the first position to the second position, map information within a predetermined range centered on the position of the moving object in the moving environment is used. Get a local map,
By map matching between the local map and the environment map representing the moving environment, the positions of the plurality of passing points in the moving environment are estimated as a plurality of estimated passing points,
Storing a set of the plurality of estimated pass points in the storage unit as a travel route;
By arranging each of the plurality of local maps acquired at the plurality of pass points at the corresponding estimated pass points, the environment map is created and stored in the storage unit,
The first local map or the second position acquired at the first position by acquiring the local map while the moving body moves in at least one of the vicinity regions of the first position or the second position. Expanding at least one range of the second local map acquired at,
Calculating a relative positional relationship between the first position and the second position based on a shift amount between the first local map and the second local map in which at least one range is enlarged;
A control unit;
A moving route acquisition device comprising:   The moving route acquisition device according to claim 9, wherein the control unit enlarges a range of the second local map.   The moving route acquisition device according to claim 9, wherein the control unit enlarges a range of the first local map. The control unit acquires the local map as a set of observation points representing the positions of obstacles existing in the predetermined range,
Determining whether a first difference between the number of observation points included in the first local map and the number of observation points included in the second local map is larger than a first threshold,
When the absolute value of the first difference is larger than the first threshold, the range of the second local map is expanded.
The moving route acquisition device according to claim 9. The control unit acquires the local map as a set of observation points representing the positions of obstacles existing in the predetermined range,
It is determined whether the number of the observation points included in the first local map and / or the number of the observation points included in the second local map is smaller than a second threshold value,
Expanding the range of the first local map and / or the second local map in which the number of observation points is smaller than the second threshold value;
The moving route acquisition device according to claim 9. The control unit acquires the local map as a set of observation points representing the positions of obstacles existing in the predetermined range,
It is determined whether there is a gap larger than a predetermined size between adjacent observation points included in the first local map and the second local map,
The observation point group existing between the two gaps is extracted as a cluster,
Determining whether the absolute value of a second difference between the number of clusters included in the first local map and the number of clusters included in the second local map is greater than a third threshold value,
When the absolute value of the second difference is larger than the third threshold, the range of the second local map is expanded,
The moving route acquisition device according to claim 10. The control unit includes:
Calculating a third difference between the number of observation points included in one cluster of the first local map and the number of observation points included in one corresponding cluster of the second local map;
Determining whether the absolute value of the third difference is greater than a fourth threshold,
When the absolute value of the third difference is larger than the fourth threshold, the range of the second local map is expanded.
The moving route acquisition device according to claim 14. The control unit includes:
It is determined whether the number of the observation points included in the first local map is smaller than a second threshold,
Expanding the range of the first local map when the number of observation points included in the first local map is smaller than the second threshold value;
The moving route acquisition device according to claim 14.

Images