JP6801269B2 - Autonomous mobile device - Google Patents

Description

The present invention is a moving body (robot vehicle or automatic guided vehicle (AGV)) that operates while measuring the surrounding environment using a two-dimensional laser range finder (LRF), and the distance from the LRF when the moving body is tilted. It relates to an autonomous mobile device that corrects data.

A moving object that operates while measuring the surrounding environment using a two-dimensional LRF performs two-dimensional map generation and self-position / attitude estimation based on the distance data from the LRF (the "position / attitude" in this case is Since it is two-dimensional, it has a position (x, y) and a posture θ (= orientation on a horizontal plane). However, when map generation and self-position attitude estimation are performed based on the distance data from the LRF, the attitude of the moving object (the "attitude" in this case is three-dimensional, so roll, pitch, and yaw (ψ, φ, respectively) If (θ) is tilted (roll ψ and pitch φ change), this distance data will be distorted, and the accuracy of map generation and self-position attitude estimation will decrease.

Japanese Patent No. 5141507 Japanese Patent Application No. 2015-225493

In Patent Document 1, the map generation and self-position / posture estimation based on the distance data from the LRF are temporarily suspended when the moving body is tilted or while the moving body is tilted (self-position / posture estimation). Is performed using odometry information from the drive system), which discloses a technique for suppressing a decrease in accuracy.

However, with this technology, when the road surface is tilted for a long time, such as when there are irregularities over a long distance, or when the fluctuation of the tilt continues for a long time, map generation / self-position posture based on the distance data from the LRF Estimates will not be made for a long time. As a result, especially for map generation, there is a possibility that the generation accuracy may decrease and the map may be divided.

In view of the above-mentioned technical problems, the present invention makes two-dimensional map generation and self-position / orientation estimation highly accurate even when the moving body is tilted and while the tilt of the moving body is fluctuating. It is an object of the present invention to provide an autonomous mobile device that can be performed.

The autonomous mobile device according to the first invention that solves the above problems is
It is an autonomous mobile device installed in a moving body equipped with a distance sensor that measures the distance to an object on the moving path.
A distance data acquisition unit that acquires distance data measured by the distance sensor,
A position / posture change amount measuring unit that acquires the movement amount and the posture change amount of the moving body on the horizontal plane, and
A tilt measuring unit that measures the roll angle and pitch angle of the moving body,
A distance data correction unit that corrects the distance data based on the roll angle and the pitch angle to obtain the corrected distance data,
A local map generator that generates a local map of the current measurement range by the distance sensor based on the corrected distance data.
A global map generator that generates a global map based on the local map, the self-position of the moving body, and the self-posture.
The data updated by reflecting the movement amount and the posture change amount on the self-position and the self-posture immediately before is corrected based on the global map and the corrected distance data, and the latest self-position and the self are corrected. It is characterized by having a self-position posture estimation unit that obtains a posture.

The autonomous mobile device according to the second invention that solves the above problems is
In the autonomous mobile device according to the first invention.
The distance data correction unit
The distance data is corrected by using the roll angle and the pitch angle to obtain the corrected distance data.

The autonomous mobile device according to the third invention that solves the above problems is
In the autonomous mobile device according to the second invention,
The distance data correction unit
The measurement points obtained from the distance data at equal intervals of measurement angles are projected onto a horizontal plane based on the roll angle and the pitch angle with reference to the center point of the distance sensor, and the points of the measurement points projected onto the horizontal plane. The intersection of the line connecting the groups in order and the straight line extending from the center point of the distance sensor in the direction of each measurement angle is obtained, the distance from the center point of the distance sensor to the intersection is calculated, and this is calculated as described above. The feature is that the corrected distance data is used.

The autonomous mobile device according to the fourth invention that solves the above problems is
In the autonomous mobile device according to the third invention.
The distance data correction unit
It is characterized in that data in which the height of the measurement point from the horizontal plane before the horizontal plane projection is out of a predetermined range is deleted.

According to the autonomous moving device according to the present invention, map generation and self-position / posture estimation can be performed with high accuracy even when the moving body is tilted and while the tilt of the moving body is fluctuating.

It is a block diagram explaining the structure of the autonomous mobile device which concerns on Example 1 of this invention. It is a flowchart explaining operation until "corrected distance data" of the autonomous movement device which concerns on Example 1 of this invention is calculated. It is a conceptual diagram which shows the distance data acquired in the state which LRF is tilted in Example 1 of this invention. FIG. 5 is a conceptual diagram showing a state in which distance data acquired in a state where the LRF is tilted in Example 1 of the present invention is projected onto a horizontal plane. It is a conceptual diagram which shows the state which connected the adjacent measurement points projected on the horizontal plane in Example 1 of this invention. FIG. 5 is a conceptual diagram showing a state in which a straight line is extended at an angle ψ _{i about the} LRF in the first embodiment of the present invention to obtain an intersection. It is a flowchart explaining operation until "corrected distance data" of the autonomous movement device which concerns on Example 2 of this invention is calculated. It is a schematic diagram explaining the positional relationship between a measurement point and a predetermined range (upper limit value and lower limit value) in Example 2 of the present invention. (A) indicates a state in which the measurement point is within the predetermined range, and (b) indicates a state in which the measurement point is outside the predetermined range.

Hereinafter, the autonomous mobile device according to the present invention will be described with reference to the drawings in Examples.

[Example 1]
First, the configuration of the autonomous mobile device according to this embodiment will be described with reference to FIG. FIG. 1 is a block diagram illustrating a configuration of the correction system.

The autonomous moving device according to the present embodiment is provided on a moving body (robot vehicle or automatic transport vehicle (AGV)) equipped with an LRF as a distance sensor for measuring the distance to an object on the moving path. LRF distance data acquisition unit 11, position / attitude change amount measurement unit 12, tilt measurement unit 13, distance data correction unit 14, local map generation unit 15, global map generation unit 16, self-position / orientation estimation unit 17, and storage unit 18 is provided.

The LRF distance data acquisition unit 11 acquires the distance data measured by the two-dimensional LRF and stores it in the storage unit 18.

The position / posture change amount measuring unit 12 acquires the movement amount and the posture change amount of the moving body on the horizontal plane (two-dimensional) from the driving part of the moving body, and stores this data (position / posture change amount data). Store in part 18.

The tilt measuring unit 13 measures the three-dimensional posture of the moving body from the information of the gyro sensor, the acceleration sensor, and the like, and stores the data (tilt data) of the tilt (roll angle and pitch angle) of this posture to the storage unit 18. store.

The distance data correction unit 14 corrects the distance data acquired by the LRF distance data acquisition unit 11 based on the inclination data measured by the inclination measurement unit 13, and stores this data in the storage unit 18 as "corrected distance data". ..

The local map generation unit 15 generates a local map of the current measurement range by LRF based on the corrected distance data corrected by the distance data correction unit 14, and stores it in the storage unit 18. The "local map" is a two-dimensional map of the range currently visible in the LRF, and the moving body moves in the desired direction (direction to go) based on information such as obstacles in the range. It is a map used for. The specific procedure for generating the local map is as described in Patent Document 2 above.

The global map generation unit 16 refers to the local map generated by the local map generation unit 15 and the self-position / posture data (self-position / self-posture data of the moving body on the horizontal plane) at the time of local map generation. A global map is generated based on (described later) and stored in the storage unit 18. The "global map" is, for example, a two-dimensional map of the entire facility such as a factory, and is generated by connecting the above "local maps". The mover knows where he is and where he is on this "global map" and decides the route to his destination. Further, the specific procedure for generating the global map is as described in Patent Document 2 above.

At the start, the self-position / attitude data on the horizontal plane is set to, for example, coordinates (x, y, θ) = (0,0,0) (or special coordinates may be set), and the global range is set. Start creating the map.

In the self-position / posture estimation unit 17, the data updated by reflecting the position / posture change amount data in the immediately preceding self-position / posture data is matched with the global map generated by the global map generation unit 16 and the corrected distance data. By doing so, the latest self-position / posture data is generated and stored in the storage unit 18. The specific procedure for generating the self-position / posture data is as described in Patent Document 2 above.

Next, the operation until the "corrected distance data" of the autonomous mobile device according to this embodiment is calculated will be described with reference to the flowchart of FIG.

In this embodiment, it is assumed that "many obstacles in the surrounding environment of the moving body are vertical (extended in the vertical direction) such as a wall", and the inclination data acquired from the inclination measurement unit 13 is used. The distance data acquired by the LRF distance data acquisition unit 11 is projected onto a horizontal plane for correction.

In step S1, the LRF distance data acquisition unit 11 acquires the distance data from the LRF, and in step S2, the inclination measurement unit 13 acquires the inclination data. Further, the following steps S3 to S10 are performed by the "distance data correction unit 14".

In step S3, the measurement points obtained from the distance data are projected onto the horizontal plane with the center point of the LRF as a reference based on the inclination data. This step S3 will be described in detail below.

FIG. 3 is a conceptual diagram showing the distance data acquired in a state where the LRF is tilted. Each measurement point p _i whose measurement angle (ψ _i ) can be obtained from distance data at equal intervals is p _i (ψ _i , l _i ) (however, the front is ψ _i ) in the polar coordinate system based on the center point of the LRF. = 0,0 ≦ i ≦ N) and the when, in the x-y-z coordinate system relative to the center point of the LRF a _{(l i cosψ i, l i} sinψ i, 0).

となる。 If the moving body is inclined in the roll angle φ pitch angle theta, in the coordinate system the x-y plane is based on the center point of the horizontal LRF, the position of the p _i after the rotation _{(x i, y i, z} i) Is

Will be.

FIG. 4 is a conceptual diagram showing a state in which the distance data (FIG. 3) acquired in a state where the LRF is tilted is projected onto a horizontal plane. The coordinates of the tilted distance data projected onto the horizontal plane are
_{p'i (l i cosθcosψ i +} l i sinθsinφsinψ i, l i cosφsinψ i) a.
The height from the horizontal plane is l _i cos θ sin φ sin ψ _{i −} l _i sin θ cos ψ _i .
The above is the description of step S3.

In step S4, the above i is set to 0. In step S5, if i is less than N, the process proceeds to step S6, and when i becomes N, the operation ends (according to step S10 below, each time the cycles of steps S6 to S9 below are ordered, i becomes 0, 1, 2, 3, ..., N).

In step S6, as shown in the conceptual diagram of FIG. 5, a line segment connecting adjacent measurement points projected on a horizontal plane and intersecting a straight line extending from the center point of the LRF in the angle ψ _i direction within this line segment. To explore.

In step S7, if there is a corresponding line segment in step S6, the process proceeds to step S8 below, and if there is no corresponding line segment, the process proceeds to step S9.

In step S8, as shown in the conceptual diagram of FIG. 6, the intersection of the line segment in step S6 and the straight line extending in the angle ψ _i direction from the center point of the LRF is obtained, and from the center point of the LRF to the intersection. The distance is calculated, the "corrected distance data" of the angle ψ _{i is used} , and the process proceeds to step S10 below.

Equidistant That is, in step S6 to S8, p'point projected on a horizontal plane in the step S3 _i, the point p'i _{+ 1} of the next, the angle viewed from the center point of the LRF, due distortion rotated Is no longer. Therefore, the intersection of the line connecting the points of the distance data projected on the horizontal plane in order and the straight line extending from the center point of the LRF in the direction of the angle ψ _i is obtained, and the distance from the center point of the LRF to the intersection is calculated. , "Corrected distance data".

In step S9, when there is no corresponding line segment in step S7, the distance data in the ψ _i direction is set to “-1”, and the process proceeds to step S10. Note that this "-1" indicates that there is no distance data, and is not used for map generation or self-position / orientation estimation (distance data is deleted).
Then, in step S10, the value of i is moved up by one to move to step S5.

That is, in this embodiment, it is assumed that "many obstacles in the surrounding environment of the moving body are vertical (extended in the vertical direction) such as a wall", and the roll of the inclination data acquired from the inclination measurement unit 13 Using the angle and pitch angle, the distance data acquired by the LRF distance data acquisition unit 11 is projected onto a horizontal plane for correction.

More specifically, the distance data correction unit 14 projects measurement points obtained from distance data having equal measurement angles onto a horizontal plane with reference to the center point of the LRF based on the roll angle and pitch angle of the moving body. Find the intersection of the line connecting the points of the measurement points projected on the horizontal plane in order and the straight line extending from the center point of the LRF in the direction of each measurement angle (ψ _i ), and from the center point of the LRF to the intersection. The distance is calculated and used as "corrected distance data".

According to this embodiment, the deterioration of the accuracy at the time of map generation and the accuracy of self-position / posture estimation caused by the distortion of the LRF distance data due to the inclination of the moving body can be improved, so that the moving body is tilted. Map generation and self-position / orientation estimation can be performed with high accuracy even when the moving body is tilted or the inclination of the moving body is fluctuating.

[Example 2]
In the case of the first embodiment, even if the inclination of the moving body is small, obstacles having a large difference in height may be detected at a long distance. For example, when the search direction of the LRF is downward, a distant road surface may be detected.

In this embodiment, when performing the same processing as in Example 1, using the height of the front projecting into the horizontal plane relative to the center point of the LRF (height z _i of the measurement point p _i), the height If is out of the predetermined range, extra detection can be deleted by not using it as distance data.

FIG. 7 is a flowchart illustrating the operation until the “corrected distance data” of the autonomous mobile device according to the present embodiment is calculated. As shown in FIG. 7, step S8 described in the first embodiment is changed to the following steps S8'-1 to S8'-4. Since the other steps are the same as those in the first embodiment, steps S8'-1 to S8'-4 will be mainly described below. Further, these steps are performed by the "distance data correction unit 14" as in the first embodiment.

In step S8'-1, the intersection of the line segment in step S6 (see Example 1) and the straight line extending in the angle ψ _i direction from the center point of the LRF is obtained, and the distance from the center point of the LRF to the intersection is obtained. It is calculated, and this is used as the distance data of the angle ψ _i , and the process proceeds to step S10 (see Example 1).

In Step S8 '-2, and calculates the front horizontal projection of the intersection point calculated in step S8'-1, the height from the horizontal plane (height z _i of the measurement point p _i).

In step S8'-3, it is determined whether or not the height obtained in step S8'-2 is within a predetermined range. Here, FIG. 8 is a schematic diagram for explaining the positional relationship between the measurement points and the upper limit value and the lower limit value in the predetermined range in this embodiment, and A in the figure shows a tilted moving body. Within height z _i is a predetermined range of the measurement points p _i as shown in FIG. 8 (a), to the following step S8 '-4, the height of the measurement point p _i as shown in FIG. 8 (b) If z _i is out of the predetermined range, the process proceeds to step S9 (see Example 1).

That is, in addition to the case where it is determined in step S6 (see Example 1) that there is no line segment, in the above step S8'-3, even when the height calculated in the above step S8'-2 is out of the predetermined range. , The distance data is set to "-1" (step S9), it is assumed that there is no distance data, and the data is deleted.

In step S8'-4, the distance from the center point of the LRF to the intersection (on the horizontal plane) obtained in step S8'-1 is calculated, and this is used as the "corrected distance data" of the angle ψ _i .

That is, in this embodiment, in addition to the process described in the first embodiment, by setting a predetermined range (upper limit value and lower limit value) for the height from the horizontal plane when the moving body is tilted, the horizontal plane of the measurement point is set. By deleting the data when the height from the road is out of the predetermined range from the data projected on the horizontal plane as extra data (for example, detection of the road surface), the road surface that can be driven is detected as an obstacle. Can be prevented.

The present invention is suitable as an autonomous moving device that corrects distance data from the LRF when the moving body is tilted in a moving body that operates while measuring the surrounding environment using the LRF.

11 LRF distance data acquisition unit 12 Position / posture change amount measurement unit 13 Tilt measurement unit 14 Distance data correction unit 15 Local map generation unit 16 Global map generation unit 17 Self-position / posture estimation unit 18 Storage unit

Claims (2)

It is an autonomous moving device installed in a moving body equipped with a distance sensor that measures the distance to an object on the moving path.
A distance data acquisition unit that acquires distance data measured by the distance sensor,
A position / posture change amount measuring unit that acquires the movement amount and the posture change amount of the moving body on the horizontal plane, and
A tilt measuring unit that measures the roll angle and pitch angle of the moving body,
A distance data correction unit that corrects the distance data based on the roll angle and the pitch angle to obtain the corrected distance data,
A local map generator that generates a local map of the current measurement range by the distance sensor based on the corrected distance data,
A global map generator that generates a global map based on the local map, the self-position of the moving body, and the self-posture.
The data updated by reflecting the movement amount and the posture change amount on the self-position and the self-posture immediately before is corrected based on the global map and the corrected distance data, and the latest self-position and the self are corrected. and a self-position and orientation estimation unit for obtaining the position,
The distance data correction unit
Using the roll angle and the pitch angle, the distance data is corrected to obtain the corrected distance data.
The measurement points obtained from the distance data at equal intervals of measurement angles are projected onto a horizontal plane based on the roll angle and the pitch angle with reference to the center point of the distance sensor, and the points of the measurement points projected onto the horizontal plane. The intersection of the line connecting the groups in order and the straight line extending from the center point of the distance sensor in the direction of each measurement angle is obtained, the distance from the center point of the distance sensor to the intersection is calculated, and this is calculated as described above. An autonomous moving device characterized in that it is corrected distance data . The distance data correction unit
The autonomous moving device according to claim 1 , wherein the data in which the height of the measurement point from the horizontal plane before the horizontal plane projection is out of a predetermined range is deleted.

Images