JPS6234784A - Method of controlling movement of moving body - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed description of the invention] (b) Purpose of the invention [Industrial application field] Used for movement control of mobile robots, etc.

[Prior Art 1] Mobile robots perform tasks that require movement, which are dangerous or difficult for humans to perform, in place of humans.

Therefore, when designing a mobile robot, - To avoid collisions with other moving objects, such as people, cars, and other mobile robots, it is important to
Movement of a moving body that can control its own movement between l and V! It is essential to develop methods for this purpose.

[Problems to be Solved by the Invention] Conventionally, however, it has been considered to detect stationary obstacles and avoid collisions with these obstacles, but it has not been possible to respond sufficiently quickly to such moving obstacles. Effective movement system with 0
The I17'I method had not been developed.

This invention was made in view of the above circumstances, and if a mobile robot detects a moving obstacle, which is another moving object, while moving, and continues to move, it will collide with that moving object. The present invention provides a movement control method for a moving object that can extremely quickly estimate whether or not the collision is a collision, and based on the estimation, can control the movement speed such as acceleration, deceleration, direction change, etc. for collision avoidance. is intended to provide.

(B) Structure of the Invention Means for Solving Problem E] Corresponding to this object, a movement control method for a moving body according to the present invention is a movement control method in which a moving body avoids collision with another moving obstacle. The moving object is considered to have a graphic shape consisting of points equidistant from its center position A, and the moving obstacle is assumed to have a graphic shape consisting of points equidistant from its center position B. Measure the relative position and relative velocity of point B as seen from point A and estimate the radius r2 of the moving obstacle, and if the inner product of the relative position and relative velocity is negative, these measured values・Using the estimated value and the radius r1 of the moving O body, calculate 51 the distance between point A and the lIJ line that corresponds to the road when point B moves straight from the relative position at the relative speed, and calculate D. r1 + "Compare 2, D is r 1 +
The present invention is characterized in that when D is smaller than r 2 , the speed of the moving body is changed from time to time so that D becomes larger than r 1 + r 2 .

Hereinafter, details of the present invention will be explained with reference to the drawings showing one embodiment.

First, consider a case where a mobile robot as a moving object and a moving obstacle move in a two-dimensional space. That is, the mobile robot and the moving obstacle are considered to be planar figures that move on a fixed plane parallel to the walking surface, and collision avoidance is considered.

First, the shape of the mobile robot I~ is approximated by a circle with radius r1.

This circle may be a circumcircle of the mobile robot, or a circle with a radius that includes the mobile robot.

On the other hand, a moving obstacle is also approximated by a circle with radius r2.

Here, the shape and size of the rest of the moving obstacle are determined by the mobile robot 1.
cannot be detected in ~. That is, the method for a mobile robot to detect a moving obstacle is to emit laser light or ultrasonic waves from one light emitting point A of the mobile robot 1 to the space around the moving robot 1-, as shown in FIG. A device that emits a thin beam 2, scans it while rotating around the light emitting point A, and estimates the presence or absence, size, and position of a moving obstacle 3 based on the presence or absence of reflection, the central angle θ of the reflection part, and the time required for reflection. Therefore, it is impossible to detect the shape and size of the shadow portion 3a where laser light or the like cannot be reflected. Therefore, assuming that the rest of the moving obstacle is a circle, the radius r2 is estimated to be larger based on the size of the reflected part of the laser beam, etc., and the moving obstacle 3 is approximated by a circle with radius r2. It is something.

The momentary position of the moving obstacle 3, that is, the position of the center of the circle with the radius r2 (X2(1), V2(1)), and the velocity 2(X'2(j), y'2(j) )teeth,
1q is determined by the measurement using the laser beam, etc., but the problem is that the radius r1, the center position vector 6 equations (XI, Vl), and the speed 1 = a S +
A circle C1 that moves at b j and approximates the mobile robot 1, and a circle C2 that moves at a radius r2, a center position vector U (x2.y2), and a speed 2=ci+dj and approximates the moving obstacle 3.
is replaced with the problem of checking whether or not there will be a collision if the mobile robot 1 continues forward. Here, i and j are vectors of size 1 in the X@ and yNh directions, respectively, and (xl-x2) + (yl-y2)2> (r1
+r2).

Furthermore, instead of considering the speeds of both, only the relative speed of the moving obstacle as seen from the mobile robot is considered, and the speed of the mobile robot is 0 and the speed of the moving obstacle is = v2-1 = (c-a)i+ (d -b) Even if we consider that j = ki + IIU, the relationship as to whether or not they collide is the same. This solid 1~le and relative position vector'
For KTJ, if 7n·>0 (where * does not represent an inner product), that is, if the obstacle B is moving lυ in the direction away from the robot Δ, there is no need to check for collision. In the case of AB-vkuO, it is necessary to check in detail. In that case, set the radius of the mobile robot to 0,
That is, even if we consider the mobile robot to be a point and the radius of the object to be r=r1+r2, the distance dAB between them does not change, so the relationship as to whether or not they will collide is the same (FIG. 5).

Therefore, the problem is that when the relative position vector of point B is 100 and the relative velocity is 100/<0, the center B(x2.y
2) When circle C with radius r=r1+r2 moves at a speed of -2-1, the area that falls on the path of circle C, that is, the point B (x2.y2) shown with diagonal lines in Fig. 5, is a moving obstacle. The distance from the straight line m that corresponds to the course when moving at a relative speed of is r
A point △(xl, Vl) in the band-shaped area L1 that is
The problem comes down to checking whether or not it is included.

The equation of straight line m is m:j! (x X2) k (V V2)
= 0, so the distance between the straight line m and the point A (xl, yl) is D = l 1 (xl-x2) -k (yl-y,,
) 115T 7 = Ik(y2 Vl)-j (X2 Xl) I
/2"...(1) If D≦r, then if the mobile robot continues as it is, it will collide, [)
>r, it is estimated that the mobile robot will continue to move forward and will not collide.

In the case not shown in Fig. 5, D<r, and if the mobile robot continues as it is, it will collide, but if the mobile robot stops immediately, the path of circle C will be in the direction of 2, and the dashed lines p1, l
) the inner area sandwiched by 2.

Since this does not include points, collisions are avoided.

In the case of FIG. 5, the mobile robot does not necessarily have to stop, and the collision can also be avoided by decelerating, that is, by reducing the speed to 3, which has a magnitude smaller than V in the same direction as 1. That is, assuming that the relative speed of the moving obstacle is 1-3, the straight line Q1. After setting the inner area L1 sandwiched by q2 to be the path of the circle C, it is sufficient to make the point A not included in the area L1.

Collisions can also be avoided by increasing speed. That is, the speed is increased to 4, the relative speed of the moving obstacle is set to 1-4, and the two-point gt line S1. The inner region L2 sandwiched by S2 is circle C
If the path is set to be the path of , it is only necessary to make sure that point A is not included in region L2. If these decelerations and speed increases result in a sudden deceleration or speed increase that is impossible for the mobile robot, a direction change is performed. In fact, in the case of FIG. 5, if the direction is changed and the speed is increased to 5, the speed remains unchanged without increasing or decreasing, and the course area of circle C becomes L2, which is the same as when the speed is increased to v4.

Therefore, detection of a moving obstacle shall be performed when the distance from the moving obstacle that the mobile robot is expected to encounter in its movement space is a distance that allows the above-mentioned deceleration, speed increase, or direction change. do.

By the way, there is inaccuracy in the above estimation due to the fact that both are approximated by circles, but if it is possible to move while making this estimation from time to time, as it approaches a moving obstacle, it will become invisible to the mobile robot. It is clear that the correct estimation can be obtained in real time, since the shadowed part will also be visible to the mobile robot before it becomes the object of the collision.

Now, in equation (1), (k, j) represent the components of the relative velocity vector of the moving obstacle as seen from the mobile robot, and (X2 Xl , V2 Vl ) is the position MA (×1.yl ) is the coordinate of the moving obstacle when the coordinate axis is translated in parallel so that it becomes the origin, that is, it is the relative position of the moving obstacle as seen from the mobile robot.That is, if D= l x' (t) y(t) -y' (t)
x(t) l (2) According to equation (2), with a simple system configuration as shown in FIG. 1, it is possible to determine whether or not there will be a collision at high speed in real time. (However, the differentiation shown in FIG. 1 is not necessary if the relative velocity is obtained from a separate sensor.) This system configuration can be easily extended to three dimensions.

That is, the position A (x, y, z) centered on the mobile robot, the sphere with radius r1, and the position 1ffB (x2, y2, z2) centered on the moving obstacle.
), approximated by a sphere with radius r2, and set as . Inner product of relative velocity vector and position vector P=
x(t) x' (t) +V(t) V' (t)+
Collisions are possible only if z(t) z' (t) is negative. At that time, the straight line corresponding to the course when point B moves at the relative speed of the moving obstacle as seen from the mobile robot: (x-x2)/x' (t) = (y-y2)/y'(1) = (z z2 ) / z' (t) and point A (xl
, yl, zl)...(3) Therefore, if D > r 1 + r 2, there is no collision, D≦r
1 + r 2, it is estimated that there will be a collision, as shown in FIG.

The flowchart for the mobile robot's computer to actually execute the movement control method for avoiding moving obstacles described above is shown in the Navigation for Moving Obstacles (naV).
iQatiOn) as shown in FIGS. 6(a) and (b).

To move a mobile robot, landmarks (1 and ++ ark) are usually set along the planned movement route, and this movement path is divided into several short sections separated by landmarks. This landmark is moved as a target in each section, that is, as a subcall.

Therefore, FIG. 6 is marked as navigation between subcalls. The detected moving obstacle is dealt with by slowing down, stopping, or speeding up, but if none of these is safe, the vehicle changes direction and designs an escape route to the subgoal. Yes to “Is it safe?” here.
When determining No, the determination method of the present invention shown in FIG. 1 is used. If you reach the rib goal, E x ITP
Then move your target to the next landmark according to your schedule.

If it is an unavoidable moving obstacle, go to E

FIG. 7 shows an overall flowchart including the navigation shown in FIG. 6 as a part.

(C) Effects of the Invention As is clear from the above explanation, according to the present invention, a method for controlling the movement of a moving object that can quickly control its own movement in real time in response to the movement of other moving objects is obtained. be able to.

[Brief explanation of drawings]

FIG. 1 is a flowchart showing a method for determining the possibility of collision with a moving obstacle, which is the basis of the method for controlling the movement of a moving body according to the present invention, in the case of a two-dimensional space. A flowchart showing a method for determining the possibility of collision with a moving obstacle, which is the basis of the movement control method, in the case of a three-dimensional space; FIG. 3 is a plan explanatory diagram showing how a mobile robot detects a moving obstacle; Figure 4 is a plan view of a moving robot and a circular approximation of a moving obstacle; Figure 5 is a moving robot]
・An explanatory diagram showing the relationship between the relative position/velocity of a moving obstacle and the possibility of collision as seen from ・ FIGS. This figure is a flowchart showing the entire navigation including the moving obstacle handling navigation shown in FIG. 1... Mobile robot 2... Beam 3...
Moving obstacles Figure 1 Figure 2

Claims (1)

[Claims] A movement control method for a moving body to avoid collision with another moving obstacle, the moving body being considered to have a shape of a figure consisting of points equidistant from its center position A, and the moving obstacle being Point B seen from point A when it is considered to have the shape of a figure consisting of points equidistant from the center position B
and estimate the radius r_2 of the moving obstacle, and if the inner product of the relative position and relative speed is negative, then these measured values/estimated values and the radius r_2 of the moving object
1, calculate the distance D between point A and the straight line that corresponds to the course when point B moves straight from the relative position at the relative speed, compare D with r_1+r_2, and if D is smaller than r_1+r_2, D is smaller than r_1+r_2. A movement control method characterized by performing an operation of changing the speed of the moving body from time to time so that it becomes greater than r_1+r_2.