JP5604117B2 - Autonomous mobile - Google Patents

Description

  The present invention relates to an autonomous mobile body including an autonomous traveling type such as an unmanned vehicle and an autonomous walking type such as a robot.

Conventionally, as this type of prior art, there is one disclosed in Patent Document 1 under the name “self-propelled vehicle”.
The self-propelled vehicle described in Patent Document 1 is provided with a vehicle body provided with traveling means, and a distance from the traveling road surface that is provided in the vehicle body and is one-dimensionally scanned in the vehicle width direction in the traveling road surface in the traveling direction. A plurality of distance measuring devices that perform scanning in synchronization with each other and whose scanning planes are parallel to each other, and data extraction means for extracting appropriate distance data from the distance data acquired by each distance measuring device And road surface average line calculating means for calculating a road surface average line that is an intersection line between the traveling road surface where no obstacle exists and the scanning surface in each distance measuring device, using the distance data extracted by the data extracting means, Road surface section that calculates the cross-sectional shape of the traveling road surface in a predetermined direction of the scanning range based on the distance from the distance measuring device to the road surface average line corresponding to each distance measuring device and the installation interval of each distance measuring device The height of the obstacle is calculated based on the cross-sectional shape of the traveling road surface in the predetermined direction calculated by the shape calculating means, the road surface cross-sectional shape calculating means, and the distance data in the predetermined direction measured by one distance measuring device, Obstacle determination means for calculating the approximate position of an obstacle that is determined to be difficult to travel, as well as determining whether or not the vehicle can travel based on the inclination of the road surface or the height of the obstacle, and an outline of the obstacle calculated by the obstacle determination means And a vehicle control device that controls traveling of the vehicle main body in a manner that avoids an obstacle based on the position.

JP 2000-181541 A

By the way, as described in Patent Document 1 described above, in order to travel on rough terrain such as an unpaved road, the shape of the road surface is measured in detail in order to avoid unevenness that becomes an obstacle. Need to be evaluated.
And an obstacle map is produced | generated from the evaluated result, a route plan is performed using it, and it runs autonomously. The obstacle map is updated while sequentially reflecting the external measurement results.

FIG. 6 is an explanatory diagram showing the measurement principle of the road surface shape using the laser range finder. In the figure, reference numeral 1 denotes an autonomous vehicle, and reference numeral 2 denotes a laser range finder (LRF).
The principle of distance measurement used in the laser range finder 2 is based on the time-of-flight method for measuring the time from the projection to the reception of the laser beam. For example, one or several laser light sources are used in the lateral direction. This is a type that performs three-dimensional scanning by optically or mechanically sweeping the optical axis for performing line scanning at high speed and swinging it in the vertical (tilt) direction. Hereinafter, the horizontal direction is referred to as “line scan”, and the vertical swing is referred to as “frame scan”.

  When measuring the road surface shape, the angle formed by the road surface and the optical axis of the laser range finder 2 becomes smaller especially when the road surface is far away. Therefore, when the optical axis scan of the laser range finder 2 is performed, the line scan rate of the LRF currently generally usable is about 100 Hz. As a result, the autonomous vehicle 1 in the frame scan direction, particularly on the road surface. From the perspective, the measurement in the perspective direction is very sparse.

Here, consider a case where there is a moving obstacle such as a pedestrian. 7A and 7B are explanatory diagrams of an obstacle map in a grid map format in plan view of the surroundings of the autonomous vehicle 1.
As a result of detecting the obstacle based on the measurement result by the laser range finder 2, among the moving areas of the obstacle map M, the area considered to be present is black, the travelable area is white, and the unmeasured area The area is shown in gray.
Note that a region obtained by combining regions displayed in white, black, and gray is referred to as a “moving region Ma”.

As shown in FIG. 5A, there is a moving obstacle in one scan of the laser range finder 2, and at the next scan, the moving obstacle moves from the region (coordinate position) P1 to the region P2. Suppose you are.
For an area once detected as an obstacle, after the moving obstacle moves and leaves, it is necessary to remeasure the road surface of the area again in order to evaluate that the area can travel again.

However, since the measurement of the road surface tends to be sparse, re-measurement of these areas is performed for the first time by performing scanning a plurality of times.
Therefore, after the moving obstacle moves, the area is not updated as an obstacle until those areas are remeasured.

  This appears on the map in a form called a ghost as shown in P3 to P5 so that the obstacle is pulled in the moving direction as shown in FIG. When such ghosts occur frequently, it is difficult to plan a correct movement route, and there is a problem that efficient traveling cannot be performed.

  Accordingly, an object of the present invention is to provide an autonomous mobile body that can perform efficient autonomous movement by removing a ghost of a moving obstacle in a timely manner regardless of the movement mode of the moving obstacle.

An autonomous mobile body according to the present invention for achieving the above object is
In an autonomous mobile body having a distance measuring unit for acquiring distance measurement data in a moving region and a driving mechanism for autonomously moving in the moving region based on the distance measuring data acquired by the distance measuring unit The distance measuring unit is a laser range finder that performs distance measurement by scanning the laser beam in the horizontal direction and performing frame scanning by swinging the optical axis for performing the line scan in the vertical direction. Based on the acquired several line scan data, an obstacle detection unit that detects an obstacle that hinders autonomous movement, and determines whether the obstacle detected by the obstacle detection unit is a moving obstacle. a moving obstacle determining unit, a map creation means for creating a map including the moving obstacle data relating to the determination of the moving obstacle determining means, moving obstacle de on the map map producing means And marking means for setting a moving obstacle flag the time corresponding to the rate of the frame scanning as a marking to the coordinate position of the motor as the life, before time corresponding to the rate of the frame scanned from when the marking, the marking coordinates The moving obstacle detecting means for redetecting the moving obstacle at the position, and when the moving obstacle detecting means redetects the moving obstacle, the life of the moving obstacle flag set on the map is extended and the frame scan rate is increased. If a moving obstacle is not redetected even after the time corresponding to elapses, a moving obstacle erasing means for erasing the moving obstacle flag set on the map, and a map in which the moving obstacle flag at the re-detected coordinate position is left. Based on the route plan creation means for creating a route plan, and in accordance with the created route plan, it is transferred via the drive mechanism. And a moving means for moving the area.

  According to the present invention, efficient autonomous movement can be performed by removing the ghost of the moving obstacle in a timely manner regardless of the movement mode of the moving obstacle.

It is explanatory drawing which shows schematic structure of the autonomous mobile body which concerns on one Embodiment of this invention. It is a block diagram of the control circuit provided in the autonomous mobile body same as the above. It is explanatory drawing which shows the mode of distance measurement when the obstruction which faced the front of the unmanned vehicle exists. It is a control flowchart when an autonomous mobile body same as the above performs autonomous traveling. It is a flowchart which shows the detail of step 8 shown in FIG. It is explanatory drawing which shows the measurement principle of the road surface shape using a laser range finder. (A), (B) is explanatory drawing of the obstacle map of the grid map format which planarly saw the mode of the autonomous vehicle periphery.

  EMBODIMENT OF THE INVENTION Below, the form for implementing this invention is demonstrated with reference to drawings. FIG. 1 is an explanatory diagram showing a schematic configuration of an autonomous mobile body according to an embodiment of the present invention, and FIG. 2 is a block diagram of a control circuit provided in the autonomous mobile body.

An autonomous unmanned vehicle A as an autonomous mobile body according to an embodiment of the present invention includes an action control computer 10, a map creation / route planning computer 20, and sensor data connected to each other via an Ethernet (registered trademark) 5. The processing computer 30 controls the entire vehicle.
Hereinafter, the “autonomous unmanned vehicle” is simply referred to as “unmanned vehicle”.

The sensor data processing computer 30 includes a CPU (Central Processing Unit), an interface circuit (none of which is shown), and the like, and includes a laser range finder 31, a vertical gyro 32, and an odometry 33 via a serial communication circuit. A global positioning system 34 is connected.
In the following description, the laser range finder is abbreviated as “LRF”, and the global positioning system is abbreviated as “GPS”.

The LRF 31 performs distance measurement by a time-of-flight method for measuring the time from projecting to receiving light of a laser beam. In the present embodiment, the one shown in this embodiment uses one laser light source and the optical axis is optical or By scanning mechanically, as shown in FIG. 6, a scan type of scanning the laser beam in the horizontal direction and scanning the laser beam in the vertical direction to obtain the three-dimensional shape of the object. Is.

The vertical gyro 32 obtains optical axis posture (orientation) information, and outputs, for example, posture angles (roll, pitch angle and angular velocity), yaw angular velocity, and X, Y, and Z3 axis accelerations. Yes.
The odometry 33 is a sensor for acquiring own position information based on the rotation amount of the wheel 35 of the unmanned vehicle A.
The GPS 34 is for acquiring positioning information of the unmanned vehicle A.

The sensor data processing computer 30 described above exhibits the following functions by executing a required program.
(1) Obstacles that hinder autonomous movement based on distance measurement data (each several line scan data: see st1 in FIG. 4 ) acquired by a distance measurement unit (hereinafter also referred to as “LRF”) 31. The function to detect. This is referred to as “obstacle detection means 30a”.

(2) A function of determining whether an obstacle that hinders autonomous movement is a moving obstacle. This function is referred to as “moving obstacle determination means 30b”.
Hereinafter, taking as an example a case where there is an obstacle facing forward, whether or not it is a moving obstacle by acquiring distance measurement data will be described. FIG. 3 is an explanatory diagram illustrating a state of distance measurement when an obstacle facing the front of the unmanned vehicle A exists.
In the present embodiment, it is assumed that what moves on the road surface such as a person or a vehicle is a moving obstacle and has a surface facing the optical axis when viewed from the LRF 31.

  If the front of the unmanned vehicle A is a flat surface, the distance measurement value of the line scan distance measurement data subjected to the frame scan largely changes according to the tilt angle in the same yaw direction. Specifically, assuming that the height of LRF 31 (distance from the road surface T to the rotation center of the optical axis) is h, the tilt angle of the optical axis is θ, and the distance measurement value by LRF 31 is r, the distance measurement value ra with the ground. = H / sin (θ), and ra changes greatly when the tilt angle θ of the optical axis is small.

  On the other hand, when the obstacle P is directly facing the LRF 31, if the horizontal distance from the LRF 31 to the target facing the object is L, the distance measurement value rb = L / cos (θ) with the obstacle becomes the distance measurement value rb. The amount of change with respect to the tilt angle θ of the optical axis is small. Here, when the distance measurement value is differentiated by the tilt angle, it can be expressed by the following formulas 1 and 2.

そして、それぞれの比を取ると、式３で表すことができる。

And if each ratio is taken, it can represent with Formula 3.

The absolute value of the ratio of the change ratios is large when the tilt angle θ of the optical axis is small, and becomes 130 when the tilt angle θ of the optical axis is 5 ° and 205 when θ = 4 °.
Therefore, when the rate of change of the distance measurement value with respect to the tilt angle in the frame is reduced by about 1/100, these areas are considered to be directly facing objects (obstacles).

  However, since the tilt angle θ of the optical axis is an angle of the optical axis with respect to the road surface T assumed to be horizontal, a value corrected by the vertical gyro 32 is used. Further, since the distance measurement value also changes depending on the direction of the LRF 31, it is preferable to evaluate using a value obtained by correcting the posture of the data to be compared based on the yaw rate.

  Regarding the change in the translation position of the vehicle, when the line scan rate of the LRF 31 is about 100 Hz and the vehicle speed is 30 km / h, the movement amount between one line scan is 100 mm or less, and the influence on the evaluation is small. . If the evaluation greatly affects the scan rate and vehicle speed, correction is performed.

Further, when the height h of the LRF 31 is 1 m, the rate of change is about 1/10 in the vicinity region of about L = 3 m, and further changes if the vicinity L = 1 m, that is, the tilt angle θ of the optical axis is 45 °. The rate is 1.
However, since the measurement point density on the road surface T is naturally dense in such a vicinity, the ghost can be frequently deleted without performing the above processing.

Note that it may be determined whether the tilt angle when the distance measurement data is acquired is equal to or less than a threshold value.
For example, the height of the LRF 31 is set to 1 m from the ground, and the ratio of the amount of change in the distance measurement value is set to 25 so that an obstacle directly facing 5 m ahead can be detected. That is, an obstacle can be detected when the obstacle directly facing is 5 m or more, but cannot be detected when there is an obstacle before 5 m.

  That is, when Atan (1/5) = 11 ° or more in terms of the tilt angle, it is impossible in principle to detect an obstacle facing directly, but the tilt when the distance measurement data described above is acquired. By determining whether or not the angle is equal to or less than the threshold value, unnecessary processing (calculation amount) can be reduced, and processing in a shorter time can be performed.

(3) A function of marking the coordinate position on the map of moving obstacle data. This function is referred to as “marking means 30c”.
Specifically, the moving obstacle flag is set as a marking at the coordinate position on the map where the moving obstacle data exists. Note that the map in the present embodiment is conceptually equivalent to the map described in FIG. 7 above, and the area indicated by the map M is used as the movement area, and therefore the description thereof is omitted.

(4) A function of redetecting a moving obstacle at the marked coordinate position before a predetermined time elapses. This function is referred to as “moving obstacle detection means 30d”.
The “predetermined time” is a time corresponding to the frame scan rate (time for one round trip up and down) from when the marking is performed.
Since the moving obstacles that face each other are stably measured in each frame scan, the life of the moving obstacle flag to be marked on the map is preferably set to the same level as the frame scan rate. Thereby, the occurrence of ghost can be suppressed to the same level as the frame scan rate.
In other words, it is a function for confirming whether or not there is a moving obstacle at the marked coordinate position before a predetermined time elapses.

  The map creation / route planning computer 20 includes a CPU (Central Processing Unit), an interface circuit (none of which is shown), and the like, and exhibits the following functions by executing a required program.

(5) A function of creating a map including moving obstacle data corresponding to the moving obstacle according to the determination. This function is referred to as “map creation means 20a”.
The “map including moving obstacle data” is also referred to as “obstacle map”.

(6) When a moving obstacle is re-detected at the marked coordinate position, the moving obstacle data (moving obstacle flag) at the re-detected coordinate position is left every time a predetermined time elapses, and other coordinates A function to delete the moving obstacle data (moving obstacle flag) of the position. This function is referred to as “moving obstacle erasing means 20b”.

In other words, if the marking on the map is erased after the moving obstacle is detected, even if the moving obstacle can always be detected by scanning, the movement obstacle will Occurs in a state where does not exist on the map. Therefore, it affects the action plan and route plan of unmanned vehicles.
Therefore, the moving obstacle is re-detected at the marked coordinate position before the predetermined time elapses, and the moving obstacle data (moving obstacle flag) at the re-detected coordinate position is left every predetermined time elapse. In addition, the moving obstacle data (moving obstacle flag) at other coordinate positions is deleted, and the movement route is planned.

(7) A function of creating a route plan based on a map in which moving obstacle data (moving obstacle flag) at the re-detected coordinate position is left. This function is referred to as “route plan creation means 20c”.
As a result, even if the moving obstacle stays at the same coordinate position, a smooth moving path can be generated, and the unmanned vehicle A can be moved smoothly and efficiently. Become.

(8) A function of replacing terrain data based on distance measurement data when the moving obstacle data (moving obstacle flag) does not exist at the coordinate position on the map from which the moving obstacle data (moving obstacle flag) is deleted. . This function is referred to as “data replacement means 20d”.
The distance measurement data at the time when there is no moving obstacle is sequentially stored in a storage area (not shown) in the map creation / route planning computer 20, for example. Thereby, the coordinate position from which the moving obstacle is erased can be brought into a natural undulation state.

  The behavior control computer 10 includes a CPU (Central Processing Unit), an interface circuit (none of which are shown), and the like, and the behavior control computer 10 operates a handle, an accelerator, a brake, and the like of a general passenger vehicle. Thus, various actuators are connected via an output-side interface, the details of which are as follows.

That is, a drive mechanism C such as a steering actuator 12 and a brake / accelerator actuator 13 is connected to the behavior control computer 10 via a motor driver 11, and the following functions are executed by executing a required program. Demonstrate.
(9) A function for moving in the moving area via the drive mechanism C in accordance with the created route plan. This function is referred to as “moving means 10a”.

  Next, a control flowchart will be described with reference to FIGS. FIG. 4 is a control flowchart when the unmanned vehicle A according to an embodiment of the present invention autonomously travels, and FIG. 5 is a flowchart showing details of step 8 shown in FIG.

  Step 1 (abbreviated as “st1” in the figure. The same applies hereinafter): Distance measurement data is received from the LRF 31. Here, it is assumed that several pieces of buffered data can be accessed in order to evaluate a change in distance measurement data in a plurality of line scan data.

  Step 2: The distance measurement data acquired by the LRF 31 is converted from the sensor coordinate system to the vehicle body coordinate system with the vertical upward as one axis by the vertical gyro 32. In order to consider turning of the unmanned vehicle A, the coordinate system is based on the position and direction where the first one of the plurality of line scan data is acquired.

Step 3: Check whether the distance measurement value at each coordinate position is greater than or equal to the threshold value.
Such a distance measurement value is data in the vicinity of the unmanned vehicle, and since the amount of change in the distance measurement value is small, if it does not meet this condition, the determination process of whether it is an obstacle directly facing is not performed, Step 9 described later is performed.

Step 4: The change of the distance measurement value with respect to the tilt angle of the optical axis at each coordinate position is evaluated.
Specifically, for the data after coordinate conversion, the amount of change in the distance measurement value with respect to the amount of change in the tilt angle is calculated with respect to the data in the yaw direction. Since the value is a discrete value, it becomes a difference. At this time, if the influence of the difference error becomes large for data with a very small change in tilt angle, it is excluded from the evaluation.

Step 5: It is determined whether or not the amount of change in the distance measurement value is equal to or less than a threshold value. When it is determined that the area is equal to or less than the threshold value, the area is evaluated as an obstruction facing the object. In other cases, the process of step 9 described later is performed.
For example, the height of the LRF 31 is set to 1 m from the ground, and an obstacle facing the object is detected 5 m ahead.

  Here, according to the above equation 3, when an obstacle facing directly ahead is measured 5 m ahead, the ratio of the change amount of the distance measurement value is 1 / (0.2 ^ 2) = 25. That is, when the obstacle directly facing is 5 m or more, the amount of change is 25 or more, and when the obstacle directly facing is 5 m or earlier, it cannot be detected with this threshold value.

  If the threshold value is reduced, theoretically it is possible to detect nearby obstacles directly facing, but in practice, erroneous detection frequently occurs due to measurement errors and the like. This is mostly due to the swing of the LRF optical axis and the shape of the moving region (such as undulations).

On the other hand, if the threshold value is increased in order to reduce false detection, only detection can be performed only at a farther distance, and a dead zone occurs in the vicinity.
For example, in order to detect a near-facing obstacle, the height of the sensor (LRF 31) is set low.

Step 6: Estimate the height of the area determined to be an obstacle directly facing. As a technique, the objects that existed in the vicinity of the confronting objects estimated from the amount of change in the distance measurement value are collected, and the difference between the highest point and the lowest point in the height direction of the measurement point is taken to obstruct the problem. The height of the object.
Next, the height assuming the moving obstacle is set as a threshold value and compared. This is because, when it can be assumed that an object with a low height is a fixed obstacle, it does not become a ghost generation source. The threshold is determined in consideration of the size of the assumed moving obstacle. For example, assuming the size of a person, assuming a bent state, for example, the height is set to 500 mm or more.

  Step 7: In the LRF measurement point data determined to be a moving obstacle facing directly, a flag (moving obstacle flag) indicating that the movement obstacle is present in a flag register (not shown) provided in any computer. Set.

Step 8: On the map generation application, normal fixed obstacles hold past measurement data until new measurement data is obtained, but about one frame has elapsed for data with moving obstacle flags After that, erase the data on that grid.
If there is past distance measurement data, it is restored. In other words, the terrain data based on the distance measurement data when the moving obstacle data (moving obstacle flag) does not exist is replaced with the coordinate position on the map from which the moving obstacle data (moving obstacle flag) is deleted.

Details of step 8 will be described with reference to FIG.
Step 8-1 (abbreviated as “st8-1” in the figure. The same applies hereinafter): The grid position (position on the map) corresponding to the position of the LRF measurement point on the grid map (map) is coordinated Extract from

Step 8-1: Check whether the moving obstacle flag is set in the data of the LRF measurement point.
Step 8-3: When the moving obstacle flag is not set in the data of the LRF measurement point, the obstacle detection result obtained in the process of st9 in the flowchart shown in FIG. 4 is written in the grid data.

Step 8-4: When the moving obstacle flag is set in the data of the LRF measurement point, is the moving obstacle flag already set on the grid corresponding to the position of the data of the LRF measurement point (in the grid )? Check if there is already a moving obstacle) .
Step 8-5: If the moving obstacle flag is not already set on the grid, the moving obstacle flag is set, and a timer indicating the life of the flag is started. The timer time is about one frame.
In addition, the possibility that the obstacle disappears from the map can be reduced by lengthening the set time of the timer. However, in the case of a moving obstacle, there are many ghosts on the map.
On the other hand, if the set time is shortened, ghosts on the map can be reduced. However, the possibility of losing an obstacle on the map increases.

Step 8-6: When the moving obstacle flag is already set on the grid, the moving obstacle flag timer is restarted to extend the life of the moving obstacle flag. The extended time is about one frame, which is the same as the original timer time.
Step 8-7: For the grid where the moving obstacle flag is set, the moving obstacle flag is deleted for the grid whose timer has expired. At the time of this erasure, if there is terrain data measured in the past, it is restored.

Step 9: It is a normal obstacle detection algorithm, and an obstacle is detected from irregularities and gradients based on geometric information obtained from distance measurement data obtained by the LRF 31.
By repeating the above steps (sequence) continuously and continuously generating a map, a map with less ghost is generated.
According to the present invention described in detail above, it is possible to smoothly and efficiently perform autonomous movement by removing ghosts of moving obstacles in a timely manner.

The present invention is not limited to the above-described embodiments, and the following modifications can be made.
In the above-described embodiment, the unmanned vehicle is described as an example in which the entire unmanned vehicle is controlled by the three computers including the behavior control computer, the map creation / route planning computer, and the sensor data processing computer. You may make it control by a single computer, without making a computer share.

10a moving means 20a map creating means 20b moving obstacle erasing means 20c route plan creating means 20d data replacing means 30b moving obstacle judging means 30c marking means 30d moving obstacle detecting means 31 ranging unit A autonomous moving body (unmanned vehicle)
C Drive mechanism M Map Ma Moving area

Claims (2)

In an autonomous mobile body having a distance measuring unit for acquiring distance measurement data in a moving region and a driving mechanism for autonomously moving in the moving region based on the distance measuring data acquired by the distance measuring unit ,
The distance measuring unit is a laser range finder that performs distance measurement by performing a frame scan that vertically scans the optical axis for performing line scanning while performing line scanning of the laser light in the horizontal direction,
Obstacle detection means for detecting an obstacle that hinders autonomous movement based on several line scan data acquired by the distance measuring unit,
A moving obstacle determining means for determining whether the obstacle detected by the obstacle detecting means is a moving obstacle;
Map creating means for creating a map including moving obstacle data related to the determination by the moving obstacle determining means;
Marking means for setting a moving obstacle flag having a lifetime corresponding to the frame scan rate as a marking at the coordinate position of the moving obstacle data on the map created by the map creating means;
Moving obstacle detection means for redetecting a moving obstacle at the marked coordinate position before the time corresponding to the frame scan rate has elapsed from when marking was performed,
When the moving obstacle detection means redetects the moving obstacle, the life of the moving obstacle flag set on the map is extended, and the moving obstacle is not redetected even if the time corresponding to the frame scan rate elapses. Sometimes moving obstacle erasing means for erasing the moving obstacle flag set on the map ,
A route plan creation means for creating a route plan based on the map in which the moving obstacle flag at the re-detected coordinate position is left;
An autonomous mobile body characterized by having a moving means for moving in a moving area via a drive mechanism in accordance with the created route plan.   2. The data replacement means for replacing the topographic data based on the distance measurement data at a time when the moving obstacle flag does not exist at the coordinate position on the map from which the moving obstacle flag is deleted. Autonomous mobile body.

Images