JP2772682B2 - Travel control device for unmanned vehicles - Google Patents

Description

Description: BACKGROUND OF THE INVENTION [0001] The present invention relates to traveling control of an unmanned mobile such as a mobile robot, and more particularly to grasping the surrounding state of the unmanned mobile by using fuzzy inference and suitably detecting obstacles. BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a traveling technique for a travel control device for an unmanned mobile body that travels to a destination while avoiding the situation, in which not only the position of the destination but also a stop direction thereof is specified.

[Conventional technology]

Conventional techniques for avoiding obstacles include mathematical programming or heuristic search using a conventional configuration space. In these conventional techniques, an optimal avoidance route or an ideal curve is obtained, and a mobile robot travels along the route or the curve. Was like that. In the related art, since the steering control is performed every time the mobile robot deviates from the calculated route, there is a problem that the motion of the robot is jerky and traveling cannot be performed efficiently. Also,
In the above-described conventional technology, there is a problem that a robot cannot travel efficiently along a smooth curve because a route cannot be intuitively selected as in the case of a human driving a car.

By the way, when the traveling control of the unmanned mobile body is performed, not only the position of the target point but also the stop direction of the mobile body at the target point may be specified. In such a case, conventionally, in consideration of the position and the direction of the vehicle at the starting point and the position and the direction of the target point, ideally, it is possible to make a reasonable travel between these two points without the need for a sharp steering angle control. A curve is set in advance, and the speed and the steering angle of the vehicle are controlled along this route.

[Problems to be solved by the invention]

In such a conventional method, when the target point changes, a new route must be obtained each time, and the calculation load is large.
There is a problem of inefficiency.

In the method using the ideal curve, when an obstacle is present in the route, the position of the obstacle is detected in advance,
It is necessary to calculate a route that allows the vehicle to travel comfortably while taking the position of the obstacle into consideration, which is disadvantageous in that the calculation becomes more complicated.

After all, the route from the starting point to the target point should not be uniquely determined, but it is preferable that the vehicle be run in the same way as a human being who can freely determine it according to the situation while the vehicle is running.

The present invention has been made in view of such circumstances,
When the position of the target point and the stop direction at the target point are specified, the state around the unmanned mobile object is accurately understood, and the unmanned mobile object is selected by performing the same route selection as that based on human intuition. An object of the present invention is to provide a travel control device for an unmanned mobile object that travels efficiently along a smooth curve.

[Means for solving the problem]

According to the present invention, position and orientation measurement means for sequentially measuring the current position and orientation of the unmanned mobile object, target point information setting means for setting the position of the target point and the stopping direction of the unmanned mobile object at the target point, Distance measuring means for sequentially measuring the distance to obstacles existing around the omnidirectional at predetermined angular intervals in all directions, and a line extending in the stop direction through the target point according to the set stop direction.
1. A second temporary target point is selected, and virtual obstacles are automatically set on both sides of a line connecting the target point and the second temporary target point near the target point. The pseudo setting means and the position and orientation measuring means, the target point information setting means, the distance measuring means and the predetermined fuzzy inference based on the output of the pseudo setting means perform the vehicle speed and steering angle control of the unmanned mobile object to thereby control the obstacle and the obstacle. Fuzzy control means for causing an unmanned mobile to travel to the target point via the first temporary target point and the second temporary target point while avoiding a virtual obstacle is provided.

[Action]

In the present invention, travel control is performed using fuzzy inference. As the information on the target point, not only the position but also the stop direction is specified.

When the unmanned mobile body starts traveling, first, the first and second unmanned moving bodies are drawn on a straight line extending in the designated stopping direction through the target point.
Is selected, and virtual obstacles are automatically set on both sides of a line connecting the target point and a second temporary target point close to the target point. Then, first, the unmanned vehicle is caused to travel with the first temporary target point, which is farther from the target point (true goal), out of the two set temporary target points as the destination.

Then, assuming that the unmanned mobile arrives at the first temporary target point while avoiding the obstacle, the unmanned mobile object now travels with the second temporary target point on the side closer to the true goal as the destination. I do.

Further, assuming that the unmanned mobile body reaches the second temporary target point, the unmanned mobile body is caused to travel with the set target point (true goal) as a destination.

Then, when the unmanned moving body approaches within a predetermined distance from the goal, deceleration control for stopping the moving body is started. As a result, the moving object reliably stops at the set target point in the set stop direction.

In the above control, the fuzzy rules for vehicle speed and steering control are shared in all traveling from the starting point to the destination.

〔Example〕

An embodiment of the present invention will be described below with reference to the accompanying drawings.

In this embodiment, the present invention is applied to a mobile robot. The mobile robot 1 has a pair of left and right drive wheels 2 and four casters 3, as shown in FIG. By being driven, the vehicle travels. These drive wheels 2 are each driven by a motor 6, as shown in FIG. A pulse encoder 5 is provided on the rotation axis of each motor 6, and output pulses of these pulse encoders 5 are input to a current position measuring unit 10.

The current position measuring unit 10 calculates the rotation speeds of the left and right driving wheels 2 from the outputs of the pulse encoders 5 and sequentially calculates the current position (X, Y) and the posture angle φ of the robot 1 in the absolute coordinate system based on these rotation speeds. Calculation output. In this case, a known dead reckoning is used for this calculation.

A range finder as an obstacle data measurement unit 20 is provided on the upper part of the robot body. The range finder 20 is configured as shown in FIG.

That is, the laser beam from the laser oscillator 21 is
Is reflected and output to the outside. Since the mirror 22 is rotated about the axis 24 by the motor 23, the laser light is emitted in all directions at 360 °. Pulse encoder 2
5 outputs a pulse corresponding to the rotation of the motor 25, and inputs this pulse to the counter 26. The counter 26 counts the pulses to calculate a rotation angle of the mirror 24, that is, a laser projection angle signal θ indicating the laser projection direction, and inputs the angle signal θ to the calculator 30. The laser projection angle signal θ is a signal indicating the angle between the reference direction and the laser projection direction, with the direction in which the mobile robot 1 is facing as the reference orientation.

On the other hand, the reflected light of the emitted laser light from the obstacle is received by the PSD 28 (semiconductor position detecting device) via the optical system 27. The PSD28 converts the signal a indicating the light transmission position into an A / D converter
It is input to the arithmetic unit 30 via 29.

The arithmetic unit 30 sequentially calculates the distance r between the mobile robot and the obstacle in all directions based on the input light receiving position signal a and the laser projection angle signal θ, and calculates the calculation result as the distance r. The signal is output as a (r, θ) signal in which the laser projection angle signal θ is combined.

The details of the calculation for obtaining the distance r are not directly related to the present invention, and therefore the description thereof will be omitted, but the applicant has filed a Japanese Patent Application No. 63-1988 filed on August 10, 1988.
The technology described in 199315, "Mobile obstacle detection device" is used.

By the way, the range finder 20 outputs the distance data r in all directions. In this embodiment, the range finder 20 outputs the distance data in all directions at intervals of 5 °. That is, when the range finder 20 is rotated once, the range finder 20 outputs 72 distance data (r _i , θ _i ) as shown in FIG.
(I = 0 to 71) is output. The data of the traveling direction of the robot is θ ₀ = 0 °, and thereafter, i increases by +1 each time the rotation angle advances 5 ° in the counterclockwise direction. However, in this case, θ is given a positive or negative sign with the traveling direction of the robot as a reference direction, and θ _{1 to} θ ₃₅ > 0 and θ _{36 to} θ ₇₁ <0. That is, θ ₃₅ = 175 °, θ ₃₄ = 170 °,... Θ ₁
= 5 °, θ ₀ = 0 °, θ ₇₁ = -5 °, θ ₇₀ = -10 °, ...
... It is assumed that θ ₃₇ = −175 ° and θ ₃₆ = −180 °.

Further, in this range finder 20, there is a limit to the measurement distance r, r = r _a (e.g. r _a = 6 m) can only measure distance obstacles within the distance to the obstacle r _a
When larger or when there are no obstacles,
From the range finder 20, r = ∞ (infinity) is output.

In this case, since the range finder 20 is mounted above the vehicle body, only obstacles having a height h> h 'can be detected (see FIG. 2), but the range finder 20 is located at a lower position at the front of the vehicle body. By mounting, obstacle detection at a lower height can be achieved.

Next, in FIG. 1, the absolute coordinates (X _c, Y _c) of the target position setting unit 40 target point and stop those directions phi _c is set, the target position and the stop direction G (X _c, Y _c , φ _c )
Is set by the operator before traveling. That is, as the information on the target point, not only the position but also the stop direction is specified.

The apparatus of these obstacle data (r, theta), the current position and orientation data of the robot (X, Y, φ) and the target position and stopped direction data _{(X c, Y c, φ} c) of the mobile robot on the basis of By calculating the velocity data v and the angular velocity data β and inputting them to the motor drive unit 35, the motor drive unit 35 controls the driving of each motor 6 and allows the mobile robot to travel to the target point while avoiding obstacles. In this apparatus, the pseudo data setting unit 90, the passability determination unit 50, the pass area selection unit 60, and the travel control unit 70 are used to make the robot travel so as to stop the robot in the stop direction specified at the target point. By performing the following processes, the final output data v and β are output from the traveling control unit 70.

- pseudo data setting unit _{90 (X c, Y c,} φ c) data is input, when there is a specified stop direction phi _c is provisional goal _{GS 1 (X 1, Y 1} ), GS
₂ (X ₂ , Y ₂ ) are pseudo-set automatically, and a virtual obstacle such as a garage is automatically pseudo-set near the true goal. Hereinafter, the position data (X ₁ ,
The data (position, width, etc.) relating to Y ₁ ), (X ₂ , Y ₂ ) and the virtual obstacle are collectively referred to as pseudo setting data.

-Passability determination unit 50 (r, θ) data and (X, Y, φ) data are input,
Based on these data, the travel area is divided into areas, and it is determined whether each of the divided areas can pass or cannot pass. Further, the same passability determination processing is performed for the virtual obstacle.

• Passing area selection unit 60 (r, θ) data, (X, Y, φ) data (X _c , Y _c , φ _c )
When data and the pseudo setting data are input and there are a plurality of passable areas, one optimum passable area is selected based on the input data.

Traveling control unit 70 (r, θ) data, (X, Y, φ) data and (X _c , Y _c ,
φ _c ) data and the pseudo setting data are inputted, and the traveling steering control for causing the mobile robot to travel to the selected passable area, the provisional goals GS ₁ and GS ₂ , and the true goal G is performed.

However, the fuzzy inference unit 80 is used for the arithmetic processing in the passage area selecting unit 60 and the traveling control unit 70, and these arithmetic processings are executed by fuzzy inference.

Hereinafter, the arithmetic processing by these constituent elements 50, 60, 70, 90 will be described more specifically.

Processing in the pseudo data setting unit 90 The pseudo data setting unit 90 adds the stop direction φ _c to the input target position and stop direction data (X _c , Y _c , φ _c ).
Is designated, as shown in FIG. 5, two pseudo subgoals GS ₁ (X ₁ , Y ₁₎ are placed on the line segment E extending in the stop direction φ _c designated through the goal position G. ₁ ), GS
₂ (X _2, Y ₂₎ sets the goal G and subgoals GS ₂ and the virtual obstacle 4a such on both sides of the line segment garage connecting,
4b is set in a pseudo manner. The virtual obstacle 4a shown in FIG. 5,
4b and is intended only conceptual, in fact, passable region of the width Wb as subgoals GS ₁ and the goal G comes in the middle, in other words, is that providing the gate.

The positions of subgoals GS ₁ and GS ₂ are in the specified stopping direction φ _c
Is determined according to. That is, this case is in the stop direction phi _c as shown, approach direction f _a, of f _b, f _a
Since it is necessary to select the side, subgoal to f _a side GS _1, G
Setting the S _2. If the stop direction φ _c is 180 ° opposite, the subgoals GS ₁ and GS ₂ are set on the opposite f _b side.

Specifically, in the pseudo data setting unit 90, the stop direction φ _c
The goal position G on either the f _a side or the f _b side
(X _c, Y _c) a predetermined distance set in advance from L _a, sets the subgoal GS _1, GS ₂ to a point on the L _b distant segment E. Incidentally, subgoal GS ₁ is provided in order to identify whether to enter from either the robot 1 f _a, the f _b to the robot. The virtual obstacle 4a, 4b is subgoal GS ₁ is set at a position equidistant from the line segment E to come in the middle.

Then, the pseudo data setting unit 90 sets these subgoals GS
_1, GS ₂ of the position data _{(X 1, Y 1),} (X 2, ( and Y _2), wherein the virtual obstacle 4a, the width data W _b of 4b as a pseudo setting data, passable determination unit 50, passage The data is input to the area selection unit 60 and the traveling control unit 70.

Incidentally, in the present running control, when the stop direction phi _c is designated, first performs a travel which is destined for subgoal GS _1, then after the mobile has reached the subgoal GS ₁ subgoal the GS ₂ performs the driving which is the destination, after the moving body 1 in this subgoal GS ₂ has reached,
The vehicle travels with the true goal G as the destination. This is a great precondition for performing the actual traveling control.

That is, in the traveling control, the traveling procedure is divided into three procedures of (1) start point → subgoal GS ₁ (2) subgoal GS ₁ → subgoal GS ₂ (3) subgoal GS ₂ → goal G. Hereinafter, the details of each component will be described with reference to the main flowchart of FIG.

[· Step (1)] That is, (FIG. 6 step 300) if the specified stop direction phi _c of the goal G is made, first of all performs running control as the destination subgoal GS ₁ (step 310) . Usually, from the start point to the subgoal GS ₁ is obstacle exists, in step 310 will be performed travel control toward the subgoal GS ₁ while avoiding obstacles.

Processing in the passability determination unit 50-Area division The passability determination unit 50 uses the output data (r _i , θ _i ) (i = 0 to 71) of the range finder 20 to omnidirectionally 360 around the robot 1. The area division in the ° direction is performed as follows.

That is, the following condition determining formulas _{r i + 1 -Δr <r i} <r i + 1 + Δr (i = 0~70) r 0 -Δr <r i <r 0 + Δr (i = 71) ( However, [Delta] r: set Is satisfied, it is determined that (r _i , θ _i ) and (r _{i + 1} , θ _{i + 1} ) are in the same area, and if the above determination formula is not satisfied, (r _i , θ _i ) and (r _{i + 1} , θ _{i + 1} ) are determined to be different areas. However, when the following expression is satisfied when i = 71, it is determined that (r ₇₁ , θ ₇₁ ) and (r ₀ , θ ₀ ) are in the same region.

That is, the determination expression is distance data r _i of the adjacent measuring points, as long as there is no significant difference than Δr between r _{i + 1,} these measuring points is determined to be the same region. According to such an area division, for example, when an obstacle 4 as shown in FIG. 7 exists,
It is divided into three areas, BK ₀ , BK ₁ , and BK ₂ .

By the way, in the area division by the above equation, for example, the eighth
As shown, Despite the possible likely areas BK ₁ passes between the region BK ₀ and region BK _2, it is impossible to determine the region BK ₁ as a separate region.

Therefore, the following conditional expression is further added to the conditional expression.

_{That, r i + 1 -Δr ≧ r} i or _{r i + 1 + Δr ≦ r} i ( _{however, r i + 1 ≠ ∞ r} i ≠ ∞; i = 0~70) r 0 -Δr ≧ r i or r ₀ + Δr ≦ r _i (provided that _{r i + 1 ≠ ∞ r i} ≠ ∞; i = 71) ... if established, 2 measurement of (r _i, θ _i) and _{(r i + 1, θ i} + 1) The point is separated from the preceding and succeeding regions to generate one region formed by only these two measurement points. The addition of determination by the determination formula, even when the eighth diagram enabling the identification of regions BK ₁ between regions BK ₀ and region BK _2.

As described above, when the region division processing using the conditional expression is completed, a start point indicating the start of the region and an end point indicating the end of the region are extracted for each of the divided regions, and the obstacle data at the start point is represented by ( r _{s [j]} , θ
_{s [j]} ) and the obstacle data at the end point (r _{e [j]} ,
θ _{e [j]} ). Where j = 0 to n-1
Where n is the number of area divisions.

-Passability determination When the above-described area division processing is completed, the passability determination unit 50
Determines whether each of the divided areas can pass. Hereinafter, the determination algorithm will be described with reference to the flowchart of FIG.

First, a determination process is performed to determine whether or not it is possible to pass from the divided area of j = 0 (step 100).

Here, in this device, it is assumed that the robot 1 proceeds to the front area, and the robot 1 does not travel in the area determined to be the rear area.

Therefore, the passability determination unit 50 determines whether the divided region of j = 0 belongs to the front region or the rear region (Step 110). In this front / rear area determination, 90 ° left / right in the direction in which the vehicle is facing, that is, −
90 ° ≤ θ ≤ 90 ° as `` front '', and among the divided regions, it is determined that a divided region that does not include any part of the region in the angle range of `` front '' belongs to the rear region, All others are determined to belong to the front area.

Therefore, in the case as shown in FIG.
Of BK ₀ ~BK _5, determines that the _{BK 0, BK 1, BK 2} , and BK ₅ belong to the front region, BK ₃ and BK ₄ belong to the rear region. In the case as shown in FIG. 10B, the divided area BK ₀
It determines that all ～BK ₃ belongs to the front region.

Then, the divided area determined as the rear area by this determination is not allowed to pass, and the robot does not travel toward this rear area (step 160).

If it is determined that the divided area belongs to the front area,
The passability determination unit 50 determines the obstacle distance data of the divided area.
It is determined whether an obstacle exists in the divided area based on r _i (step 120). That is, in this step 120, if the conditional expression is not a divided area and all r of the divided area are not ∞, it is determined that the divided area is an obstacle area and the divided area is passed. It is impossible (step 160).

When it is determined in step 120 that the divided area is not the obstacle area, the passability determining unit 50 determines whether the divided area is the side wall area (step 130). That is, as shown in FIGS. 11 (a) and (b), when there is an obstacle 51 such as a wall on the left and right sides of the robot 1, the conditional expression is satisfied, and the side wall is moved to the eighth side. since confuse the gap region BK ₁ as shown in figure, so as to extract a side wall area by the determining by the following equation.

_{r s [j]> r e} [j] when r _{s [j]} to the right of r> r _{s [j]} and left next to the r of _{r e [j] <r e} [j] ... r s [ _j] <r _{e [j]} when r _{s [j]} to the right of r <r _{s [j]} and r _{e [left j]} next to the r> r _{e [j]} ... that is, the or expression If the condition is satisfied, it is determined that the divided area is the side wall area, and the passage is not allowed (step 160). The equation is for detecting the left side wall, and the equation is for detecting the right side wall.

The divided areas (r _{s [j]} , θ _{s [j]} ) to (r _{e [j]} ,
θ _{e [j]} ) is, consequently, a region where r is all ∞ as shown in FIG. 12, or a region determined as a gap region between obstacles by the conditional expression as shown in FIG. In the passage impossible determination unit 50, such an area is hereinafter referred to as “area expansion”.
(Step 140).

This area expansion processing is an operation performed to grasp the width interval Z of the area to some extent accurately. Specifically, the start point s of the divided area determined not to be an obstacle area
This is an operation to expand [j] and the end point e [j] to the left and right.

When the values of r of the divided area are all ∞ as shown in FIG. 12 during the area expansion, first, the end point e [j] and the start point s [j] of the divided area are moved to the left and right, respectively. Expand one section (5 °). That is, the suffixes of the start point and the end point of the expanded area are s '[j], e'
If [j], then s' [j] = s [j] -1 e '[j] = e [j] +1 However, if s' [j] =-1, s' [j] = 71 e' [ If j] = 72, e '[j] = 0.

When the value of r of the divided area is not ∞ as shown in FIG. 13, the area is not expanded at first, and s ′ [j] = s [j] e ′ [j] = e [j] Set it temporarily.

Then, the area expansion processing is executed according to the area expansion algorithm shown in FIG.

In passable determination unit 50, first of r _{s '[j]} and r _{e' [j]}
Comparing the door (step 200), 'a [j] (step _{210), r s' r s} '[j] ≦ r e' when a _{[j] K = e [j} ]>
If r _e ′ _[j] , K = s ′ [j] (step 2
50). That, you select the larger of the r _{s '[j]} and r _{e' [j],} continue to the region extended by the following processes only the side that said selected. _{r e '[j] ≦ r} s' when a _[j] is then r _kL> r _k or _{r kL <r s' [j} ] <r k r kL; r value next to the left of r _k ... Is checked (step 220). If this formula does not hold, k is incremented by 1 to make the area 1
It is expanded to the left side of the section (step 240), and thereafter, it is searched again whether or not the above expression holds (step 220).
Thereafter, k is +1 until the above equation is satisfied.
As a result, the area is expanded leftward by one section. Then, k when the equation is satisfied is set as the final extension point e '[j] (step 230), and the process ends.

_{r s'[j]> r} e ' when a _[j] is the reverse r _kR> r _k or _{r kR <r e' [j} ] <r k r kR; right of the r _k Until the r value... is established, the start point s' [j] is decremented by one, so that the area is extended leftward by one section.

That is, in such area expansion processing, r _s ′ _[j]
And r _e ′ _[j], the one having the shorter r value is made to expand the other region until the other r value is substantially equal, so that a somewhat accurate width interval Z can be obtained. I have. Incidentally, in FIG. 12, two partitions are finally expanded to the left and one partition is expanded to the right, and in FIG. 13, three partitions are expanded to the left.

After performing the above-described area expansion processing, the passability determination unit 50 checks the angle range of the area (FIG. 9, step 14).
1) If the angle range is greater than 180 °, it is assumed that the area can be passed as it is because there is no way to find the area width (step 17).
0), if the angle range ≦ 180 °, the width Z of the area is calculated (FIG. 9, step 142). This width Z is calculated as follows using the cosine theorem (see FIGS. 12 and 13).

Next, the passability determination unit 50 compares the calculated area width Z with the width L (known) of the mobile robot 1 (step 150). If Z> L, the pass is possible. If Z ≦ L, the pass is impossible. (Steps 160 and 170).

When the passability determination processing for j = 0 is completed in this way, the passability determination unit 50 then increments j by 1 (step 180), and performs the same passability determination processing as described above for the next divided area. Run. In the same manner, it is determined whether or not all the n divided areas can pass.

Processing at Passage Area Selection Section 60 Selection of Passage Area Assuming that m passable areas are obtained by the above-described processing by the passability determination section 50, the obstacle data at the start point and end point of the area before expansion and the data after expansion are obtained. Similarly, m pieces of obstacle data are prepared.

The passing area selecting unit 60 selects which of the m passing areas the robot 1 is to travel. The fuzzy inference unit 80 is used for this selection processing.

That is, when a person drives a car to a target point, considering that there are some obstacles on the way, if the target point can be seen beyond a certain passable area, drive the car to that area. If it is not, one passable area is intuitively determined based on whether the width of the passable area is “wide”, the distance to the area is “close”, or “close” to the target point. Will determine. As described above, the expression such as “wide” or “close” is an expression having ambiguity. In the present invention, fuzzy inference is used as a means for selecting an optimal passable area, and selection by human intuition is performed. The same control as described above is performed.

Here, as shown in FIG. 15, local coordinates centered on the robot 1 are considered, the center of the robot 1 is O, the direction of the robot is F, the direction of the subgoal position GS ₁ is GS ₁ ′, and the direction F The angle of the direction GS ₁ ′ with respect to is defined as θ _g . In addition,
In this Fig. 15, the robot 1 moves from the starting point to the subgoal GS
It is assumed that the vehicle runs up to ₁ . FIG. 15 also shows a case where the subgoal direction GS ₁ ″ is at the front right of the robot 1. The following index is used as an index indicating how much the subgoal direction GS ₁ ′ deviates from the passable area. Define the angle Θ _k .

θ _g <θ _{s [k]} or θ _g> θ _{e [k]} if _{Θ k = min (| θ s} [k] -θ g |, | θ e [k] -θ g |) θ s [k _{] <θ g <θ e [} k] if Θ _k = 0 ... Incidentally, min (a, B) is a means that selects a, the smaller value of the B.

According to the above equation, if the subgoal direction GS ₁ ′ is within the angle range of the passable area before expansion, Θ _k = 0, and in this case, this area is selected with the highest priority. This is because if the subgoal direction GS ₁ ′ is included in the angle range of the passable area, the subgoal point is closer to the area regardless of whether the area is slightly narrow or even if the distance to the area is long. Based on the idea that the distance to GS ₁ will be the shortest.

On the other hand, if Θ _k ≠ 0, the pass area is determined by fuzzy inference. That is, when a human is driving a car,
The intuitive judgment / guess that the area is wide, or the distance to the area is short, or closer to the goal direction is to select the area that seems to be the best. It quantifies the ambiguity of human thinking and uses fuzzy inference as a means to make such inferences. Then, the fuzzy inference is used to calculate a value that becomes the passable priority α _k for each passable area, and the area having the largest α _k value is selected.

Here, the width interval of each area obtained in the previous area expansion processing by the passability determination unit 50 is Z _k (in FIG. 15, And The middle point of ▲ ▼ is M, and robot 1
Is defined as the distance l _k = ▲ ▼ from the area to the passable area.

Then, the following eight fuzzy rules are set using Θ _k , l _k , and Z _k as input fuzzy variables and α _k as output fuzzy variables.

(1) If Θ _k is SMALL and l _k is NEAR and Z _k is WI
DE then α _k is VH (2) If Θ _k is SMALL and l _k is NEAR and Z _k is NA
RROW then α _k is H (3) If Θ _k is SMALL and l _k is FAR and Z _k is WID
E then α _k is H (4) If Θ _k is SMALL and l _k is FAR and Z _k is NAR
ROW then α _k is M (5) If Θ _k is BIG and l _k is NEAR and Z _k is WIDE
then α _k is M (6) If Θ _k is BIG and l _k is NEAR and Z _k is NARR
OW then α _k is L (7) If Θ _k is BIG and l _k is FAR and Z _k is WIDE
then α _k is L (8) If Θ _k is BIG and l _k is FAR and Z _k is NARRO
W then α _k is VL The evaluation of each fuzzy variable of Θ _k , l _k , Z _k , and α _k is as follows.

Θ _k : SMALL ... small BIG ... large l _k : NEAR ... close FAR ... far Z _k : NARROW ... narrow WIDE ... wide α _k : VH (very High) ... very high H (High) ... high M (Medium) ... Medium L (Low): Low VL (Very Low): Very low Also, the fuzzy variables Θ _k ,
The membership functions of l _k , Z _k , and α _k are shown.

The fuzzy inference unit 80 executes these fuzzy rules (1) to
(8) and the passability priority α _k of each divided region is obtained using the membership function, and the divided region having the largest α _k value is selected as the passage region. EMMamdan is an indirect method proposed by Yahachiro Tsukamoto et al.
Although there are various methods such as the direct method proposed by i and the like, an arbitrary method may be used.

Processing in the travel control unit 70 · Run control If the optimal area to be passed is determined by fuzzy inference by the pass area selection unit 60, the travel control unit 70 then performs control to run the robot in the determined area. Do. Furthermore, after avoiding the obstacle, performs driving control for running to subgoal GS _1. The fuzzy inference unit 80 is also used for this traveling control.

That is, when a person drives a car, for example, when the steering angle of the car is considerably deviated to the left from the passing area, and when the distance to the area is long, the steering angle of the car is slowly turned to the right. The vehicle speed and steering control are performed based on experience and judgment. Therefore, if the traveling control of the mobile robot is also performed by fuzzy inference that quantifies the ambiguity of human thinking, it is possible to travel near human driving.

In the traveling control of the procedure (1), the traveling state is divided into the following three cases, and different traveling control is performed for each case.

_{(I) θ e [k]} -θ s [k] <180 ° (ii) θ e [k] -θ s [k] ≧ 180 ° with cutlet _{Θ = 0 (iii) θ e} [k] -θ s _[k] with the travel control of ≧ 180 ° with cutlet Θ ≠ 0 (i), as a fuzzy input variable, the distance l _k to passing area described above, it is used in addition to the angle [psi _M width spacing Z _k of the passing area. Angle [psi _M, as shown in FIG. 15, ▲ is the angle orientation of the robot for ▼, is an index indicating the measure of how much the robot from the middle direction of the passage region. Also, as fuzzy output variables,
The velocity v and the angular velocity β of the robot are used.

The fuzzy rules for speed / steering control are as follows.

(1) 'if l _k is FAR then v is FAST (2)' if l _k is NEAR and Z _k is WIDE then v is FAS
T (3) ′ if l _k is NEAR and Z _k is NARROW then v is S
LOW (4) ′ if l _k is FAR and ψ _M is LLEFT then β i
s RIGHT (5) ′ if l _k is FAR and ψ _M is LEFT then β is
RIGHT (6) 'if l _k is FAR and ψ _M is M then β is M (7)' if l _k is FAR and ψ _M is RIGHT then β i
s LEFT (8) ′ if l _k is FAR and ψ _M is RRIGHT then β
is LEFT (9) 'if l _k is NEAR and ψ _M is LLEFT then β
is RRIGH (10) ′ if l _k is NEAR and ψ _M is LEFT then β i
s RIGHT (11) ′ if l _k is NEAR and ψ _M is M then β is M (12) ′ if l _k is NEAR and ψ _M is RIGHT then β
is LEFT (13) ′ if l _k is NEAR and ψ _M is RRIGHT then β
is LLEFT (14) ′ if min R _r is SHORT then β is LEFT (15) ′ if min L _r is SHORT then β is RIGHT (16) ′ if min R _rv is SHORT then v is SLOW (17) ′ if min L _rv is SHORT then v is SLOW where fuzzy variables l _k , Z _k , ψ _M , v, β, min R _r , min
The evaluation of L _r , min R _r υ and min L _r υ is as follows.

l _k : NEAR ... close FAR ... far Z _k : NARROW ... narrow WIDE ... wide ψ _k : LLEFT ... shifted to the left LEFT ... shifted to the left M ... Medium RIGHT ... shifted to the right RRIGHT ... quite v is shifted to the right: fAST ... fast sLOW ... slow β; LLEFT ... quickly turn to M ... without rIGHT ... right steering turn to lEFT ... left turn to the left RRIGHT ... turn to quickly right _{min R r = min (r i} ) i = 54~71 for the β decision _{min L r = min (R i} ) i = 1~18 for the β decision _{min R rv = min (r i} ) i = 54~71 v determined for the min L _rv = min (r _i ) I = 1 to 18 v determination Also, the membership functions indicating the grade values of these fuzzy variables ψ _M , l _k , Z _k , v and β are shown in FIGS.
(I) shows each.

That is, even in the traveling control, the fuzzy inference unit 80
Using the fuzzy rules (1) 'to (17)' and the membership functions shown in FIG. To control the speed and steering of the robot.

The traveling control (i) of the mobile robot according to the fuzzy rules (1) 'to (13)' described above does not mean that the robot is traveling along the straight line OM, but is at the center point M of the passing area. The robot is about to run (see Fig. 15).

With regard to speed control, when the distance to the point M is long, the robot is driven at high speed. When the passage area is narrow or when the distance to the point M is short, the vehicle speed is reduced and the robot is driven at low speed. Like that.

Regarding steering control, for example, if the distance to point M is long and the robot is facing far left, the steering angle should be slowly turned to the right. If the vehicle turns to the left, the steering angle is controlled so as to quickly turn to the right.

The rules (14) 'to (17)' of the fuzzy rules (1) 'to (17)' are for dealing with the case shown in FIG.

That is, the robot 1 in this case ▲ ▼ since oriented shifted by [psi _M to the left with respect to the direction, the robot 1 according to the travel control by the rules of the previous (1) '- (13)' is The steering angle is controlled by 制 御_{M to the} right, and there is a possibility that the vehicle will collide with the lower left corner of the right obstacle 4.

Therefore, the fuzzy input variables min R _r , min L _r , min R
_rv and min L _rv , and fuzzy rules (14) 'to (17)'
These are used to avoid collisions in the case shown in FIG. Note that min R _r and min L _r
Is used to control the steering angle β, and the min R _rv and min L
_rv is for controlling the speed v. min R _r , min L
_r, min R _rv and minL _rv evaluation of SHORT (short) only without defining a membership function as shown in FIG. 17 of the above as an indication of these grades value (f) ~ (i). That is, min R _r and min R _rv are mobile robots 1
Means that the minimum value of the distance data r _i of i = 54 to 71 (−90 ° ≦ θ <0 °) is selected, and min L _r and min L _rv are the left of the robot 1. it is meant that selects the minimum value of among the distance data r _i of forward or i = 1~18 (0 ° <θ ≦ 90 °).

According to such fuzzy control, if the distance between the mobile robot and the right front obstacle is short, the speed is reduced, and then the steering angle is turned to the left, and if the distance between the mobile robot and the left front obstacle is short, After the speed is reduced, the steering angle is turned to the right, thereby avoiding contact / collision with an obstacle.

Next, in the travel control of (ii), this travel control is performed by θ
_{e [k]} −θ _{s [k]} ≧ 180 ° and Θ _k = 0, that is, the subgoal direction GS in the passing area sandwiched between the θ _{s [k]} direction and the θ _{e [k]} direction ₁ 'is present.

That is, in this case, the travel control to the use of (i), since the region distance Z _k is not determined can not travel control (i) (see FIG. 19). Therefore, in this case, as shown in FIG. 19, the subgoal position GS ₁ (X ₁ ,
The distance between Y ₁ ) and the robot position O is set as the input variable l _k of the fuzzy rule, the input variable Z _k is substituted with an arbitrary large constant value, and the input variable ψ _M is substituted with -θ _g .

Ie These are used as fuzzy input variables to perform speed / steering control in accordance with the fuzzy rules (1) 'to (17)'.

According to the running control of the (ii), the robot 1 is not towards the middle direction of the passage region, will travel toward the subgoal GS _1. Further, since the set of Z _k to a value, the robot 1 is running at a high speed toward the subgoal GS _1.

Next, the traveling control of (iii) is performed when θ _{e [k]} −θ _{s [k]} ≧ 180 ° and, for example, as shown in FIG. That is, this is a case where the subgoal direction GS ₁ ′ does not exist in the passing area.

In this case, the interval Z _{k of the} area is the same as the travel control of (ii).
Is not obtained, the traveling control of (i) cannot be performed, and there is an obstacle in the direction of the goal, and (ii)
I can not do like the running control of.

Therefore, in such a case, the obstacle is passed on the left or right side.

More specifically, the following equation _{| θ s [k] -θ g} | <| θ e [k] -θ g | If ... is satisfied, it is determined that the person who has passed through the Hidarisoba is closer to the goal, Pass on the left side.

That is, to the left of the start point a ₁ after the area expansion,
The imaginary point C ₀ provided, the point C ₀ and virtual endpoints of passage area, so as to travel the robot 1 toward the center M ₁ of sandwiched between the point a ₁ and the point C ₀ region.

The virtual point C ₀ is, for example, Are satisfied.

And the fuzzy input variables l _k , Z _k , ψ _M Using these fuzzy input variables, speed / steering control is performed in accordance with the fuzzy rules (1) 'to (17)'.

On the other hand, as opposed to the formula _{| θ s [k] -θ g} |> | θ e [k] -θ g | If ... is satisfied, those who passed through the right beside it is determined that closer to the goal Pass on the right side.

That is, to the right of the start point a ₂ after the area expansion,
The same imaginary point C ₁ is provided, the point C ₁ and virtual endpoints of passage region, toward the center M ₂ of sandwiched between the point a ₂ and the point C ₁ region so as to run the robot 1 I do.

The virtual point C ₁ is the same as described above, for example. Are satisfied.

And the fuzzy input variables l _k , Z _k , ψ _M Using these fuzzy input variables, speed / steering control is performed in accordance with the fuzzy rules (1) 'to (17)'.

I. Region division II. Passability judgment of divided region III. Region selection when there are multiple passable regions IV. Travel from start point to subgoal GS ₁ by repeating the process of travel control In this case, the vehicle travels smoothly and efficiently while avoiding obstacles with situation determination and travel control close to humans.

Also, in this case, after passing through the obstacle, the robot
The traveling is performed according to the traveling control (FIG. 19) of the above (ii). In this embodiment, the width data Z _k used as the fuzzy input variables of the fuzzy rules (1) ′ to (17) ′. Is set to a large value, the vehicle speed v derived by fuzzy inference becomes a large value after passing through an obstacle and reaching the subgoal GS ₁ , and it is possible to make the robot run at high speed during this period. it can. That is, subgoal GS ₁ is provided in order to identify whether to enter the robot according to the stop direction phi _c f _a, from either side of the f _b, the robot is simply passed over this subgoal GS ₁ You do not need to slow down.

[・ Procedure (2)] With the above traveling control, the robot 1
When reaching the _1, the travel control unit 70, detects this by comparing the input data (X, Y, φ) and the position data of the sub-goals GS ₁ from the current position measuring unit 10 (X _1, Y ₁₎ and (FIG. 6 step 320), after said detection performs a travel control in the destination subgoals GS ₂ procedure (2), moving the robot 1 from subgoal GS ₁ to subgoal GS ₂ (step 330).

In the process passable determination unit 50 in the passable determination unit 50, the output of the range finder 20 (r, theta) and the virtual obstacle 4a, region dividing based on the position data of the width data W _b and subgoals GS ₂ of 4b and The passability determination process is performed in the same manner as described above, and it is determined whether a plurality of divided areas can pass. Since there is usually no obstacle near the goal, the above-described area division and passability determination processing is performed based on only the pseudo setting data by the pseudo data setting unit 90.

Processing in Passage Area Selection Unit Assuming that m passable regions have been obtained, the passage region selection unit 60 selects a passage region in which the robot 1 should travel using the fuzzy rules (1) to (8) described above. .

However, in this case, the fuzzy rules (1) to
The θ _g (see FIG. 15) for obtaining the fuzzy input variable Θ _k (formula) used in (8) is, as shown in FIG. 21, the value of the direction GS ₂ ′ of the subgoal GS ₂ with respect to the direction F of the robot. Angle. Also, the width interval of the fuzzy input variable is set as Z _k = W _b ,
Also, the distance l _k = ▲ ▼. The membership function shown in FIG. 16 is used. In this manner, the passable priority α _k is obtained in the same manner as described above, and the largest α _k
An area having a value is selected as a passable area. Although fuzzy control is performed as described above, in this case, since the sub-goal direction GS ₂ is set in the middle of the virtual obstacle 4a, 4b, the expression of theta _k value = 0, the virtual obstacle 4a, 4b Is reliably selected as the passage area.

In the processing, the cruise control unit 70 in the travel control unit 70, moving the robot 1 from subgoal GS ₁ to subgoal GS ₂ by performing a fuzzy travel control described above (i). In this case, in the fuzzy rules (1) 'to (13)' described above, l _k , Z _k , and ψ _M are used as fuzzy input variables. In this case, these fuzzy input variables are converted as follows. (See FIG. 21).

l _k = ▲ ▼ in the _{Z k = W b ψ M =} -θ g Thus, virtual obstacle 4a robot 1, 4b midpoint M, i.e. caused to travel toward the subgoal GS _2.

The travel control [and procedures (3)] above procedure (2), the robot 1 has reached the subgoal GS _2, the travel control unit 70, the input data from the current position measuring unit 10 (X, Y, φ) and the position data (X ₂ , Y ₂ ) of the subgoal GS ₂ are detected (step 340 in FIG. 6), and after the detection, the true goal G in the procedure (3) is set as the destination. performs running control, moving the robot 1 from the subgoal GS ₂ to the goal G (step 350).

The passability determination section 50 and the pass area selection section 60 perform area division, pass permission determination, and pass area selection processing as described above. In this case, the robot 1
Since the vehicle has reached GS ₂ , a region between the virtual obstacles 4a and 4b is reliably selected as a passage region.

The travel control unit 70, the moving the robot 1 by performing fuzzy running control similar (i) from subgoal GS ₂ until the true goal G. In this case, in the fuzzy rules (1) ′ to (13) ′ described above, the fuzzy input variables
Although l _k , Z _k , and ψ _M are used, these fuzzy input variables are converted and used as described below in the same manner as in the procedure (2) (see FIG. 22).

l _k = ▲ ▼ Z _k = W _b ψ _M = -θ _{g In} this way, the robot 1 is caused to travel from the subgoal GS ₂ to the midpoint M of the virtual obstacles 4a and 4b, that is, the true goal G.

On the other hand, when the travel control unit 70 detects that the robot 1 has approached within a predetermined distance from the goal G (step 360 in FIG. 6), the deceleration for stopping is started thereafter, and the vehicle speed is gradually reduced. Then, the robot 1 is surely stopped on the goal G.

Thus, this embodiment is a virtual subgoal GS ₁
And GS ₂ are set, and a passing gate of a predetermined width is provided on both sides of the sub goal GS ₂ and the true goal G, and the robot 1
The by by way of the subgoal GS _1, GS ₂ to travel to the true goal G, is obtained so as to stop the robot 1 reliably along a specified stop direction phi _c.

The gate width W _b is is the may be determined in advance appropriately in consideration of the vehicle width or the like, W _b is more desirable to set to narrow. Because the according to the fuzzy rule (2) '(3)', when the width Z _k of the passing area is wide fast vehicle speed V,
Since the width Z _k is fuzzy control to slow down the vehicle speed V when narrow and W _b is set to be narrower, subgoal GS
_This is because the vehicle speed naturally drops near ₂ . That is, in the vicinity of the subgoal GS _2, the vehicle speed is be more convenient in terms of the person who had somewhat fallen perform fine adjustment of the steering angle for stopping correctly the robot 1 along the stop direction phi _c.

On the other hand, for subgoal GS ₁ , as described above,
Widely set the width data Z _k used as the fuzzy input variables of the fuzzy rule (1) '- (17)'. By doing so, the vehicle speed v derived by fuzzy inference becomes a large value until it reaches the subgoal GS _1, the robot 1 will be running at a high speed until reaching the subgoal GS _1.

That is, the sub-goal GS ₁ is provided to identify which side of the fa or fb the robot should enter according to the stopping direction θ _c , and the robot simply passes over the sub-goal GS ₁ You don't need to slow down.

The width Z _k is not due to the actually set obstacles like a virtual obstacle 4a, 4b of the width of the subgoal GS _1, fuzzy rule (1) input variable '~ (17)' _Is simply set to a large value in calculation.

FIG. 23 to FIG. 25 each show a simulation result by the apparatus of the embodiment, and the positions of the mobile robot 1 indicated by circles are plotted at equal time intervals. According to these figures, the mobile robot is a subgoal GS
You can see that the speed gradually decreases after passing ₁ and going to goal GS ₂ . The steering angle to be directed to the specified stop direction by traveling from subgoal GS ₂ to the target point G is controlled. Also, at this time, since the speed is reduced, there is not much hunting when controlling the steering angle.

In the above embodiment, a range finder as shown in FIG. 3 is used as a means for measuring the distance from the mobile robot to the obstacle in all directions. However, the present invention is not limited to this. Another configuration, a configuration using ultrasonic waves, or the like may be adopted.

In the above embodiment, even virtual obstacle 4a between the subgoal GS ₁ to true goal G, assuming that there is an obstacle other than 4b, function by passable determination unit 50 and the passage area selecting section 60 was to exert, if true obstacle region from subgoal GS ₁ to true goal G is not present, since the operation is wasted in the previous year Step (2) and (3), pass The operation of the possibility determination unit 50 and the passage area selection unit 60 may be stopped, and the vehicle speed and the steering control of the robot 1 may be performed by fuzzy traveling control only by the traveling control unit 70.

〔The invention's effect〕

As described above, according to the present invention, the obstacle avoidance control and the traveling control of the unmanned moving object are performed using the fuzzy inference, so that the route selection of the moving object can be performed by the situation judgment close to the human sense. At the same time, it is possible to make the moving body travel along a smooth curve with a feeling similar to human driving. Further, since the robot is caused to travel to the true goal via the two subgoals set in a pseudo manner, the robot can be reliably stopped in the designated stop direction. In the present fuzzy control, a common fuzzy rule can be used for all traveling.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration example of a control system according to an embodiment of the present invention, FIG. 2 is a side view showing a configuration example of a mobile robot, FIG. 3 is a block diagram showing a configuration example of a range finder, FIG. FIG. 4 is an explanatory diagram for explaining output data of the range finder, FIG. 5 is a diagram showing sub-goals and virtual obstacles set in a pseudo manner, FIG. 6 is a diagram showing a main flowchart of the present embodiment, and FIGS. FIG. 9 is an explanatory view showing an example of area division, FIG. 9 is a flowchart showing an example of passability determination processing, and FIGS. 10 (a) and (b) explain the concept of discrimination between a front area and a rear area, respectively. Figure used for
1 (a) and 1 (b) are diagrams used to explain a manner of determining a side wall region, FIGS. 12 and 13 are explanatory diagrams showing an example of region expansion, and FIG. 14 is an example of region expansion processing. FIG. 15 is an explanatory diagram for explaining variables used in fuzzy inference, FIG. 16 is a graph showing an example of a membership function used in fuzzy inference regarding passage area selection, FIG.
FIG. 18 is a graph showing an example of a membership function used in fuzzy inference about travel control. FIGS. 18 to 22 are diagrams used to explain travel control used in the present embodiment, and FIGS. 23 to 25. 3A and 3B are diagrams illustrating running examples of the mobile robot according to the present embodiment. 1 ... mobile robot, 4 ... obstacle, 10 ... current position measuring unit,
20: Obstacle data measurement unit (range finder), 40: Target position setting unit, 50: Passable determination unit, 60: Passable area selection unit, 70: Travel control unit, 80: Fuzzy inference unit, 90: Pseudo data Setting section.

Claims (1)

(57) [Claims] An unmanned mobile traveling control device for autonomously traveling an unmanned mobile to a target location while avoiding an obstacle and stopping the unmanned mobile in a designated direction at the target location. Position and orientation measurement means for sequentially measuring the position and orientation; target point information setting means for setting the position of the target point and the stopping direction of the unmanned mobile at the target point; Distance measuring means for sequentially measuring the distance in all directions at predetermined angular intervals; and a first and a second temporary target point on a straight line extending in the stop direction through the target point according to the set stop direction. Pseudo setting means for selecting and automatically setting virtual obstacles on both sides of a line connecting the target point and a second temporary target point on the side closer to the target point; Obstacles and the virtual obstacles are avoided by controlling the vehicle speed and steering angle of the unmanned vehicle by predetermined fuzzy inference based on the outputs of the position and orientation measurement means, target point information setting means, distance measurement means and pseudo setting means. And a fuzzy control means for causing the unmanned mobile to travel to the target point via the first temporary target point and the second temporary target point.