JPH0764633A - Obstacle evading path deciding method applying step searching method - Google Patents

Abstract

PURPOSE:To attain the calculation in a simple way and a short time by means of a compact computer by sensing the geographical features within a comparatively small range, searching an evading path against an obstacle if detected, and defining the evading point as a new starting point to repetitively search the evading paths. CONSTITUTION:An unmanned mobile investigating machine 1 senses the geographical features including obstacles existing in the front traveling direction and in the comparatively short distances as the external areas by a three- dimensional topographical sensor 4. Then the machine 1 produces a local map by an external environment recognizing means 41 and plans a traveling path in a range of the local map by a local path planning means 42. In other words, the means 42 connects the starting point of the machine 1 to its target point via a straight line to set a straight path and then sets the points on this path at each prescribed step distance toward the target point. If these set points overlap the obstacles, an evading path is searched and the searched evading point is defined as a new starting point. Thus the evading paths are repetitively searched so that a path reaching the target point is decided.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention is used to determine a route that avoids an obstacle when an unmanned mobile probe (vehicle, etc.) is present between a current position and a target point. The present invention relates to an obstacle avoidance route determination method using a step search method.

[0002]

2. Description of the Related Art When an unmanned mobile spacecraft autonomously travels on an off-road terrain on the earth or on the surface of the Moon and Mars, it detects rocks, holes, and slopes that are obstacles to travel, and recognizes and moves. Technology has been developed to enable the aircraft to travel safely. Potential method, graph search method and other methods are available as the route planning method for the mobile probe to find the traveling route from the starting point to the destination.

The first potential method is to form a potential field in which an attractive force according to a distance from a destination and a repulsive force according to a distance from an obstacle are generated, and a route is created along a potential gradient. Is the way to do it.

The second graph search method is a method in which a local map is divided into grids, a graph in which runnable grids are connected is created, and then an optimum route is searched.

In addition, thirdly, it detects only the presence of an obstacle such as an extremely simple contact sensor or proximity sensor,
As a result, some routes are changed.

[0006]

However, in the first potential method described above, a path distant from the obstacle by a certain distance can be created, but on the other hand, the local minimum point is apt to occur in the potential field, and escape from the local minimum point is likely to occur. This is a problem and there is a problem that the certainty of route creation is poor.

In the second graph search method described above, by selecting a grid, it is possible to reliably search for a route that avoids obstacles, but the graph tends to become large depending on the number of grid divisions. There is a problem that the calculation takes time and a large computer is required.

Further, the third contact sensor, proximity sensor, etc. have a small amount of information, and it is difficult to determine which direction to go after detecting an obstacle, and the traveling speed of the mobile probe. There was a problem that it couldn't be so fast.

[0009]

It is an object of the present invention to obtain terrain data by sensing a terrain including obstacles by a terrain sensor mounted on a mobile probe, avoiding obstacles caught by the terrain, and route to a target point. It is possible to perform the calculation processing of the computer simply and in a short time when determining the route, and as a result, it is possible to make the computer small and to ensure the route determination for the obstacle avoidance. An obstacle avoidance route determination method by a possible step search method is provided.

[0010]

An obstacle avoidance route determination method by a step search method according to the present invention obtains terrain data by sensing terrain including obstacles by a terrain sensor mounted on a mobile probe. When deciding the route to the target point by avoiding the obstacle caught by, the first straight line path connecting the obstacle recognition stage and the starting point of the mobile probe and the target point with a straight line And a step of setting a point on the first straight line path toward a target point for each predetermined step distance L, and a step of searching an avoidance path when the set point hits an obstacle. Then, when an avoidance point is found, the step of searching for a route again using that point as a new starting point is repeatedly executed to determine a route to reach the target point.

In one embodiment, when the point to be set reaches an obstacle, a step of planning an avoidance path in a direction in which the obstacle is less spread with respect to the first straight path and an avoidance point are found. If the first avoidance point is determined, and if the first avoidance point is not found in the direction in which the obstacle is less wide, avoid the obstacle in the same direction as the obstacle center G with respect to the first straight path. Planning a new first
The step of determining an avoidance point and, when the first avoidance point is found, connecting the first avoidance point and the target point with a straight line
It is assumed that the target point is reached by repeating the steps of setting a straight route and searching for a route again using the first avoidance point as a new starting point.

[0012]

Since the terrain sensor senses a relatively small area of the terrain and searches only a limited route, the speed of calculation is fast and simple, and the computer for calculation is small and lightweight. Will be the one.

[0013]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS FIG. 2 (a) is a schematic explanatory view showing a state where a mobile probe is performing sensing, and FIG. 2 (b) is a block diagram showing a basic configuration of a route determination device mounted on the mobile probe. The unmanned mobile probe 1 in the figure is designed to be able to travel on the off-road terrain of the earth and on the surface of the Moon and Mars. Sends information on the attitude and position of the mobile probe 1 from the antenna 3 mounted on the mobile probe 1, and takes in information from the communication station of the earth 2 and artificial satellites via the antenna 3 of the unmanned mobile probe 1. Create a wide area map and wide area route for the area you are going to proceed. And, 4 is a terrain sensor mounted on the unmanned mobile probe 1, and 5 is a ground surface.

Then, as shown in FIG. 2B, the unmanned mobile probe 1 senses an external region in a predetermined area in the forward direction of the traveling direction to obtain the external terrain data, and the external terrain. An external environment recognition means 41 for acquiring local map data in the external area sensing based on the data, and a local route planning means 42 for planning a local route in the local map acquired by the external environment recognition means 41,
The movement means 43 for moving the unmanned mobile probe 1 according to the local route and the correction data for the external environment recognition means 41 by detecting the change from the position and height data due to the movement and posture change of the unmanned mobile probe 1. An inertial sensor 44 for sending is provided, and the local route planning means 42 is transmitted from a communication station with a wide area map which is on the earth and is created using a wide area map.

Then, the three-dimensional terrain sensor 4 senses the terrain including the obstacle 5 located at a relatively short distance in the external area. The obstacles 5 include rocks, holes, etc., and objects to be sensed in the external area are height, depth, slope, slope direction, geology, and the like. Thus, the external environment recognition means 41 creates a local map within a relatively short distance, and the local route planning means 42 plans the travel route within this range.

However, in this local map, the terrain sensor 4
Since the accuracy of the terrain sensor 4 is low at a position far from the terrain, the same terrain sensor 4 is used to sense only the proximity of the proximity area, and the height, width, and center distance of the obstacle are measured to measure the obstacle. Then, the obstacle avoidance route is determined for the obstacle by using the step search method.

The sensing method by the terrain sensor 4, the external environment recognition means 41, and the local route planning means 42 will be described below.
The route decision plan by will be described in detail.

Sensing Method by Terrain Sensor 4 In order to avoid an obstacle, it is necessary to three-dimensionally sense the terrain including the obstacle and acquire the terrain data. There are the following sensing methods.

(A) Moving point tracking method with a single-lens camera (b) Stereo image method with a twin-lens camera (c) Laser slit light and light cutting method with a single-lens camera (d) Time of Flight method of laser pulse (e ) Phase difference detection method for AM-modulated laser light (for (d) and (e), a two-axis scanner is used together) As an example, a laser slit light with a simple device configuration and 1
The light cutting method by the eye camera will be described.

FIG. 3A shows the principle of the sensing method by the light section method.

In the light cutting method, the three-dimensional position of the portion on which the laser slit light is applied can be measured. As shown in FIG. 3A, one point P (xo, yo,
zo), where the position of the camera 6 is Pc (O, O, zc) and the position of the laser emitter 7 is Ps (O, O, zs), the angle α between the straight line PsP and the Z axis, and the straight lines PcP and Z. By using the angle β formed by the axis, the angle βo formed by the image center line 8 of the camera and the Z axis, and the angle γ formed by the straight line PcP and the XZ plane, the values can be obtained by the mathematical expressions 1 to 3.

[0022]

[Formula 1]

[0023]

[Formula 2]

[0024]

[Formula 3]

In order to obtain the three-dimensional coordinates of each point on the terrain including the obstacle, the above-mentioned α, βo, β, γ for each point must be obtained. This can be obtained by three-dimensionally scanning the laser light using a biaxial scanner as shown in FIG.

FIG. 3B is an explanatory view showing a method for scanning the laser slit light, in which the laser light 9 emitted from the laser light emitter 7 changes its optical path by the folding mirror 10, and Y
The laser beam having a width in the Y-axis direction can be obtained by the mirror 13 that is attached to the rotary shaft 12 of the scanner 11 while being inclined. Further, since the mirror 17 attached to the rotary shaft 16 of the X-scanner 15 can also swing in the X-axis direction, it is possible to swing the laser light three-dimensionally.

In the sensing with the laser light,
In order to obtain the coordinates of each point, it is necessary to extract the laser light from the camera image. FIG. 4 is an explanatory diagram of the laser light extraction method, in which an image 16 of the image in the camera 6 which is not irradiated with the laser is captured, and next, an image 17 at every 1/60 seconds in which the laser slit light is reflected. Take the difference. This makes it possible to obtain the image 18 in which only the high-luminance portion irradiated with the laser is extracted. That is, three-dimensional topographical data having a pixel number of 256 × 256 and a height of 8 bits is acquired.

Obstacle Recognition by External Environment Recognition Means 41 Next, the external environment recognition means 41 recognizes obstacles. Here, a method of "taking out an object that exceeds a threshold value ht of a certain height" is used, as described in the concept of obstacle recognition in FIG. Threshold value ht at this time
Was determined by the ability of the traveling mobile probe 1 to overcome. Further, in order to remove the noise 19 and the like in the image data, only the objects having a specific area or more out of the extracted objects are recognized as actual obstacles. The data of each point indicating the obstacle is stored in the image memory of the image processing apparatus in the form of height data of the obstacle. From this data, information such as the center position of the area of the obstacle, the distance, and the width is calculated. One obstacle 5 is a set of n points Ok (ik, jk) (k) as shown in the obstacle recognition explanatory diagram on the image of FIG. 5B.
= 1, 2. ． ． , N) and the area center G of the obstacle
(Ig, jg) and the width Wo can be calculated using the equations 4 and 5, respectively.

[0029]

[Formula 4]

[0030]

[Formula 5]

Route Planning by Local Route Planning Means 42 FIG. 1 (a) is a conceptual explanatory diagram of the route planning by the local route planning means 42. When the external environment recognizing means 41 recognizes an obstacle, the vehicle travels next. Plan the right route. First, as a pretreatment, in order to avoid contact between the mobile probe 1 and the actual obstacle 5, an expansion treatment (Dilation treatment) is performed in advance like the expanded obstacle 5a, and after the treatment, A local map including the inflated obstacle 5a is created by the external environment recognition means 41, and the current position ao of the mobile probe 1 and the target point 20 are designated on the local map. This current position ao and the target point 20 are connected by a straight line, and points (a1, a2 ... An) are set toward the target point 20 on this straight line for each certain step distance L. It is desirable that the step distance L corresponds to the vehicle length of the mobile probe 1.

As shown in FIG. 1B, if there is no obstacle, points (a1, a2 ...) Are made on the straight line from the starting point ao to the target point 20 for each step distance L.

Next, as shown in FIG. 1 (c), when a certain point b1 on the straight line route 21 hits the obstacle 5a, the avoidance route point is searched with the same step distance L as a unit. And first, point a on the straight path 21
It is determined from 3 which one of the left and right sides of the obstacle 5a is searched. In this case, planning the avoidance route in a direction in which the obstacle 5a does not spread will reach the target point 20 in a shorter distance.

Then, the area center G of the obstacle 5a is obtained,
The positional relationship between the center G and the straight line path 21 to the target point 20 was examined, and the side opposite to the center G with respect to the straight line path 21 was set to a direction in which the spread of the obstacle was small. In FIG. 1 (c),
Since the obstacle center G is on the left side with respect to the straight path 21,
Search for avoidance route points from the right side. At this time, the four points b2, b3, b4, a4 shown in the figure are searched,
A first avoidance point a4 that does not hit the obstacle 5a is searched for. Here, each point is located on a predetermined grid point parallel to the straight line path 21.

If no avoidance route point is found in these four points, points C1, C2, C3, C on the same side as the obstacle center G with respect to the first straight route 21 are located.
Search 4 and find a new first avoidance point.

When the first avoidance point a4 is found as shown in FIG. 1 (c), the first avoidance point a4 as shown in FIG. 1 (d).
The second straight line path 22 from 4 to the target point 20 is set, the points d1, d2, d3, and a5 are searched in the same procedure to find the second avoidance point a5. These points (a1-a
5) is located on a predetermined grid point parallel to the second linear path 22. By repeating this procedure, the obstacle avoidance route to the target point 20 can be obtained in a relatively short time.

[0037]

According to the present invention, the obstacle recognizing ability is remarkably higher than that of a contact sensor for detecting an obstacle, a proximity sensor or the like which detects only the presence of the obstacle, and accordingly , The traveling speed of the mobile probe can be greatly increased.

Further, the present invention does not cause the difficulty of escape from the local minimum point, which is a problem in the potential method in the route planning method for finding the traveling route from the current position of the mobile probe to the destination. It is possible to determine the route, and the obstacle avoidance route to the target point can be obtained in a comparatively short time as compared with the graph search method. Furthermore, since the computer can be small in size, it is extremely useful for unmanned mobile probes used in satellites and planets.

[Brief description of drawings]

FIG. 1A is a conceptual explanatory diagram of a route plan showing an embodiment of an obstacle avoidance route determination method by a step search method according to the present invention. (B) is a route plan determination diagram when there is no obstacle. (C) is an explanatory view of finding the first avoidance point when there is an obstacle. (D) is explanatory drawing which finds a 2nd avoidance point when an obstacle exists.

FIG. 2 is a schematic explanatory view of a state where the mobile probe is performing external area sensing and a block diagram showing a basic configuration of a route determination device installed in the mobile probe.

FIG. 3A is a principle diagram of a laser slit light and a light cutting method by a single-lens camera. (B) is an explanatory view showing a scanning method of laser slit light.

FIG. 4 is an explanatory diagram of a laser light extraction method.

FIG. 5A is a conceptual explanatory diagram of obstacle recognition.
(B) is an explanatory view of obstacle recognition on an image.

[Explanation of symbols]

 1 Mobile probe 4 Sensor (three-dimensional sensor) 5 Obstacle 5a Inflated obstacle 20 Target point 21 First straight path 22 Second straight path ao Starting point a4 First avoidance point a5 Second avoidance point G Obstacle area Center 41 External environment recognition means 42 Local path planning means 43 Moving means 44 Inertial sensor

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI technical display location G06T 1/00

Claims (2)

[Claims] 1. When determining a route to a target point by sensing terrain including obstacles by a terrain sensor mounted on a mobile probe to obtain terrain data and avoiding obstacles caught by the terrain. , A step of recognizing an obstacle and a step of connecting a starting point and a target point of the mobile probe with a straight line to set a first straight path,
Setting a point toward a target point for each predetermined step distance L on the first straight path, and searching an avoidance path when the set point hits an obstacle,
A method for determining an obstacle avoidance route by a step search method, wherein when an avoidance point is found, a step of searching for a route again using that point as a new starting point is repeatedly executed to determine a route reaching the target point. 2. A step of planning an avoidance route in a direction in which the obstacle does not spread with respect to the first straight path when the set point hits an obstacle, and a first step when an avoidance point is found. 1 determining an avoidance point and planning an avoidance route in the same direction as the obstacle center G with respect to the first straight route when the first avoidance point cannot be found in a direction in which the obstacle spreads little Determining a new first avoidance point, and setting a second linear path by connecting a straight line between the first avoidance point and the target point when the first avoidance point is found,
The obstacle avoidance route determination method according to claim 1, wherein the step of searching for a route again using the first avoidance point as a new starting point is repeated to reach the target point.