KR20180125646A - Planning method for robot motion - Google Patents

Abstract

The present invention provides a robot motion path planning method capable of establishing a motion path plan even in a dynamic environment. The robot motion path planning method includes: a step (s100) of forming a plurality of nodes and detection nodes in a configuration space; a step (s200) of calculating an extended restitution coefficient by comparing a radius between a node and a neighboring node with a predetermined neighbor radius; a step (s300) of calculating a detection restitution coefficient by determining whether the detection node around each of the nodes is located at an obstacle; a step (s400) of updating a node position; a step (s600) of determining whether a radius difference between the node position updated in the step (s400) and the node position before the update is less than or equal to a predetermined tolerance value; a step (s700) of returning to the step (s200) when the step (s600) is not satisfied; and a step of ending the robot motion path planning method when the step (s700) is satisfied.

Description

[0001] PLANNING METHOD FOR ROBOT MOTION [0002]

The present invention relates to a method of controlling motion of a robot, and more particularly, to a method of planning a path of a robot motion so as not to collide with an obstacle in a dynamic environment.

In general, robots are industrial robots such as manipulators and conveyor robots for automation and unmanned operation on the production site. They perform dangerous work, simple repetition work, and work requiring great power on behalf of human.

Recently, however, research and development of a humanoid robot, which has a human-like joint system and provides various services coexisting with humans in work and living spaces, is actively under way.

Such a humanoid robot performs work using a manipulator which is designed to move close to the motion of an arm or a hand by an electrical and mechanical mechanism.

Most manipulators currently in use are configured with multiple links linked together. The joint of each link is referred to as a joint, and the manipulator determines the motion characteristic according to the geometric relationship between the link and the joints.

The mathematical representation of this geometric relationship is kinematics, and most manipulators have an end-effector (hereafter referred to as end effector) as a direction to perform work with kinematicscharacteristics, .

In order for a manipulator to perform a given task (for example, to catch an object), the manipulator can perform tasks from the initial position (start point) before the task is performed, that is, to the final position It is important to create a movement path for the manipulator.

A method of sampling based path planning is known as a method of planning a path connecting a starting point and a target point while satisfying constraints such as collision avoidance in which the manipulator does not collide with obstacles in the working area.

Korean Patent Laid-Open No. 10-2011-15764 discloses a related art related thereto.

In the related art, as shown in FIGS. 9 and 10, a technique of generating a path of a manipulator using a sample path planning method in a configuration space (CS) in which an obstacle or the like of a work space WS is mapped is disclosed .

However, the Sampling Based Path Planning method is limited in application to static environments where obstacles do not move, such as industrial sites.

This is because, in a dynamic environment in which an obstacle is moving, a position can not quickly respond to a constraint of processing time which takes too much time to generate a path so as not to collide with a changed obstacle.

In addition, it is impossible to have a computing ability to deal with the variation of the size and volume of the shape space CS generated by entering and exiting the obstacle into the shape space.

In recent years, there has been an increasing demand for use of robots in dynamic environments where obstacles are changed or situations where circumstances change according to time. However, as described above, there is a method to perform motion planning of robots in such a dynamic environment It is becoming more problematic.

SUMMARY OF THE INVENTION It is an object of the present invention to provide a robot motion path planning method capable of establishing a motion path plan of a manipulator so as to avoid an obstacle at a high computational speed even in a dynamic environment.

In order to solve the above-mentioned problems, the present invention provides a method of planning a moving path of a robot under a dynamic environment, wherein a plurality of nodes (qi) are arranged inside the shape space (CS) A step (s100) of forming an index node (DELTA qj) at regular intervals around each of them;

A step (s200) of comparing the radius r between the one node qi and the adjacent node qi to a predetermined neighboring radius rn to calculate an expansion / repulsion coefficient according to the following equation: s200;

Here, the node qi, the neighboring node qj, and the expansion / contraction coefficient (F ⁱ _extension ) are directional vectors.

r _ij is the distance between node (q i) and node (q _j )

η _i means a set of neighboring nodes (qj).

The repulsive potential function (f) When 0 <r ≤ rn, it is determined as an arbitrary function that is inversely proportional to the distance (r), and when 0 <r ≤rn is unsatisfied, it is determined as O.

Determining whether or not the detection node around each node is located at an obstacle, and calculating the detection counterpart coefficient by the following equation (s300);

The collision detection function (s) is determined as follows

s (q) = 1 (when an obstacle collision occurs with the detection node in the shape space (CS))

    = 0 (no obstacle collision occurred with the detection node in the shape space (CS))

Updating (S400) the position of the node (qi) using the following equation at each node (qi);

_Here, G _1, G ₂ are each expansion coefficient of restitution F ⁱ _extension (k) and the restitution coefficient F ⁱ index _detection gain (gain) ¹ for (k).

Determining whether the radius difference between the updated node qi (k + 1) and the updated node qi (qi) position qi (k) is equal to or smaller than a predetermined allowable value? );

Returning to step s200 if the step (s600) is not satisfied (s700);

If the step (s700) is satisfied, the robot motion path planning method is terminated.

In the robot motion path planning method of the present invention, the method further includes a step (s600) of updating the neighboring radius rn between the steps (s400) and (s600) by the following equation.

G is the gain value.

Pd is a predetermined value of the sum of desired repulsive forces.

P is the sum of the repulsive forces at each node.

The time difference between kth iteration and k + 1th iteration as Δt = t _{k + 1} -t _k .

r _n (t _k ) is the neighborhood radius in the kth iteration.

In the robot motion path planning method of the present invention, the rebound potential function (f) is determined by the following equation.

f = 1/2 (1 / r-1 / rn) ² [0 < r &

 = 0 [0 < r &lt; rn satisfaction]

In the robotic motion path planning method of the present invention, the steps (s200) to (s400) are characterized by being operated in a distributed thread manner for each node.

In the robot motion path planning method of the present invention, the step (S400), i.e., the step of updating the node (qi) position, updates the position of the updated node (qi) until the position of each node ) Is characterized in that it is performed in a synchronous manner in which it waits without performing a position update operation any more.

In the robot motion path planning method of the present invention, the step (s600) further comprises connecting the nodes to form an optimal path (s800) after the step (s600).

The robot motion path planning method of the present invention has an effect that the motion path planning of the robot can be derived quickly while overcoming the constraint of the processing time even in the dynamic environment.

In addition, even when the volume of the shape space CS changes due to entry and exit of an obstacle into the shape space CS, it is possible to plan the path of the robot motion appropriately and quickly.

1 is a flowchart showing a robot motion planning method according to the present invention.
2 is a diagram showing step s100 in the robot motion path planning method of the present invention.
3 is a diagram showing step s200 in the robot motion path planning method of the present invention.
4 is a diagram showing step s300 in the robot motion path planning method of the present invention.
5 is a view showing step s400 in the robot motion path planning method of the present invention.
6 is a diagram showing step s500 in the robot motion path planning method of the present invention.
7 is a diagram showing step s700 in the robot motion path planning method of the present invention.
8 is a diagram showing step s800 in the robot motion path planning method of the present invention.
9 is a diagram showing a state in which a hand of a robot moves while avoiding an obstacle.
10 is a diagram showing a situation in the prior art in which a robot motion plan is performed in a shape space (CS).

Hereinafter, embodiments of the present invention will be described with reference to the drawings. 1 is a flowchart showing a robot motion planning method according to the present invention.

The robot motion path planning method of the present invention includes the following steps.

2 is a diagram showing step s100 in the robot motion path planning method of the present invention.

In step s100, as shown in Fig. 2, a plurality of nodes qi are arranged in the shape space CS and detection nodes? Qj are arranged at regular intervals around each of the plurality of nodes qi .

In step s100 of arranging the node qi, it is preferable to arrange the node qi in a space in which the obstacle COi does not exist. However, even if the node qi is disposed in a space in which the obstacle COi exists, The node (qi) is automatically moved to a space in which the obstacle (COi) does not exist.

On the other hand, a plurality of detection nodes? Qj are arranged around the node qi so as to have a constant radius rs also in the circumferential direction.

Although FIG. 2 shows only the case of being formed in the circumferential direction in two dimensions for the explanation of this embodiment, it may be formed as a hypersphere having n dimensions.

3 is a diagram showing step s200 in the robot motion path planning method of the present invention.

Next, the step s200 of searching for the neighboring node qi and calculating the expansion / repulsion coefficient is performed

As shown in FIG. 3, in step s300, the radius r between one node qi and the neighboring node qj is compared with a neighboring radius r _n having a predetermined value, (f) is determined as follows.

f = 1/2 (1 / r-1 / r _n ) ² [0 < r r _n satisfies]

= 0 [0 < r &_lt; rn unsatisfied]

In other words, the radius (r) is 0 <r ≤r _n repulsion potential function (f) in case of satisfying is determined by the 1/2 (1 / r-1 / r n) 2.

The rebound potential function f is formed such that the radius r between the nodes qi is distanced beyond the neighboring radius r _{n when the} radius r between the nodes qi is smaller than the predetermined neighboring radius rn And to determine the amount of movement for moving the node qi in order to make it move.

That is, in the present invention, Potential  The function (f) qi ) Is close to the radius r, the radius In the embodiment 1/2 (1 / r-1 / r _n ) ² in  It is also possible to use other expressions as long as they are functions that are inversely proportional to the distance, rather than being limited.

E.g, Repulsive potential function (f) is f = a / r Or f  = -ar or f = -ar + b or f = - ar ²  + b (where a and b are constants), etc. If the rebound potential function (f) is at the distance (r)  It is also possible to use several other well-known functions that are inversely proportional.

On the other hand, when the radius r does not satisfy 0 < r _&lt; r _n , the rebound potential function f is determined to be zero.

This is because it is not necessary to disturb the radius r between the nodes qi since the radius r between the nodes qi is larger than the predetermined neighboring radius r _n .

Since the node qi is a vector having directionality in various directions, considering the directionality, the expansion coefficient of restitution (F ⁱ _extension ) is expressed by the following equation (1).

Here, the node qi, the neighboring node qj, and the expansion / contraction coefficient (F ⁱ _extension ) are directional vectors.

r _ij Is the distance between the node (qi) and the node (qj)

η _i Means a set of neighboring nodes (qj).

In the above equation, the repulsive potential function (f) means the magnitude of the movement amount that causes the distance between the nodes (qi) to increase due to the repulsion, and (qi-qj) / r means the repulsive moving direction.

4 is a diagram showing step s300 in the robot motion path planning method of the present invention.

Next, a step (S300) of calculating the detection restitution coefficient is performed.

In step S400, the detection repulsive function s is first determined to calculate the detection repulsion coefficient (F ⁱ _detection ). The position of the detection node Qj in each node qi is determined by the shape space CS, The detection repulsive function s is determined to be 1 when it is within the range of the obstacle COi and the detection repulsion function s is determined to be 0 when it is not within the range of the obstacle COi.

That is, when s (q) = 1 (occurrence of an obstacle collision with the detection node? Qj in the shape space CS)

        = 0 (when an obstacle collision does not occur with the detection node? Qj in the shape space CS)

Since the node qi is a vector having directionality in various directions, the detection repulsion coefficient (F ⁱ _detection ) can be expressed by the following equation (2) in consideration of such a directionality.

(F ⁱ _detection ) has a negative value because the detection node (Qj) forms the detection repulsion coefficient (F ⁱ _detection ) in the opposite direction to the node (qi).

The detection restitution coefficient (F ⁱ _detection ) has a value of j = 1 ... ns since it must be calculated for all the detection nodes (Δ qj).

Here, the node qi, the node qj, and the detection repulsive coefficient (F ⁱ _detection ) are directional vectors.

S denotes a set of detection nodes (? Qj).

n _s means the number of detection nodes (Δ qj).

The collision detection function s means the amount of movement of the node qi from the obstacle when the collision with the obstacle is detected, and Δqj denotes the direction of movement when the collision is detected. will be.

Next, a step (S400) of updating the position at each node qi is performed. 5 is a view showing step s400 in the robot motion path planning method of the present invention.

The position of the node qi is updated to a new position by the following equation (3).

G _1, G ₂ are each expansion coefficient of restitution F ⁱ _extension gain (gain) ¹ for the (k) and detecting the restitution coefficient F ⁱ _index (k)

Turning to the meaning of the expression node position by the original k-th position calculation nodes qi (k) expansion coefficient of restitution at a position F ⁱ _extension (k) node position change amount, and detecting the restitution coefficient F ⁱ _index (k) by amount of variation Is calculated to express that the (k + 1) -th node position qi (k + 1) as a new position is calculated.

The node location update operation in step S400 will be described in detail with reference to FIG.

Referring to FIG. 5A, it can be seen that the neighboring node qj is disposed inside the neighboring radius r _n of one node qi, so that an expansion repelling action occurs.

Referring to FIG. 5B regarding the detection repulsion coefficient, three detection nodes Δq1, Δq2, and Δq3 among the detection nodes Δqj are located at the obstacle COi and collide with each other. Able to know.

FIG. 5C shows the contents in which the expansion repelling action and the detection repelling action act simultaneously.

Due to the occurrence of these two actions, the (k + 1) -th node position qi (k + 1) as a new node position is computed by the above-described Equation 3 as shown in FIG.

On the other hand, in step s500, the node qi proceeds in a synchronous manner such that the position of each node is all positionally updated once.

That is, in the node qi where the position has already been updated until all the positions of the respective nodes qi are updated, the process is performed in such a manner that no more position update operations are performed.

Next, the size of the neighboring radius rn is updated (S500). 6 is a diagram showing step s500 in the robot motion path planning method of the present invention.

First, the necessity to update the neighboring radius r _n will be described.

If the expansion coefficient is calculated in step s500, the radius r of the neighboring node is compared with the predetermined neighbor radius r _n .

In the dynamic environment, the obstacle (WOi) enters and leaves the work space (WS), and the size and volume of the obstacle (COi) also fluctuate in the shape space (CS) .

Therefore, if the size of the neighboring radius r _n is not appropriately changed according to the size and the volume of the shape space CS, there is a problem that the uniform distribution of the nodes is not achieved in the case of performing the robot motion path planning method do.

For example, FIG. 6 shows a state after the robot motion path planning method is repeated 100 times, where 6a is a state in which the neighboring radius r _n is 10, and no repelling action occurs.

6B shows that when the neighboring radius r _n is 30, repulsive action occurs between the nodes, but a sufficient node uniformity distribution is not achieved, such that voids are generated in the shape space CS.

FIG. 6C shows that nodes are uniformly distributed throughout the shape space CS ideally when the neighboring radius r _n is 47.

FIG. 6D shows that when the neighboring radius r _n is 60, the computation is not converged when performing the robot motion path planning method, so that a uniform node distribution is not achieved.

Therefore, setting the value of the proper neighborhood radius r _n is more and more necessary when the volume of the shape space CS is suddenly changed due to the dynamic environment.

The size of the appropriate neighborhood radius is determined by the following equation (4).

G is the gain value.

Pd is a predetermined value of the sum of desired repulsive forces.

P is the sum of the repulsive forces at each node.

The time difference between kth iteration and k + 1th iteration as Δt = t _{k + 1} -t _k .

r _n (t _k ) is the neighborhood radius in the kth iteration.

N is the total number of nodes

Next, in step s500, the node position qi (k + 1) after update is compared with the pre-position update node position qi (k) to perform step s600 of determining whether or not it is less than a predetermined allowable value? .

If the method of the present invention is repeatedly performed, the expansion restitution coefficient and the collision detection coefficient between the nodes qi are continuously calculated, the position of the node qi is constantly updated and changed, and finally the radius between the nodes qi is sufficiently secured A node (qi) position that does not collide with an obstacle is also secured.

In this state, since the position of the node qi does not change much any more, whether the change of the position of the node qi is smaller than the predetermined allowable value? Is determined as shown in Equation (5).

7 is a diagram showing step s700 in the robot motion path planning method of the present invention.

In step s700, when the difference between the node positions before and after the update becomes larger than the predetermined allowable value?, The process returns to step s200 to repeat step s200 to step s500 .

If the step S700 is continuously performed, the distribution uniformity of the nodes in the shape space CS gradually increases.

For example, when only the step s100 is initially performed, the nodes are unevenly distributed in the shape space CS as shown in Fig. 7A.

If the step (s700) is repeated five times, as shown in FIG. 7B, the uniform distribution between the nodes is gradually secured. If the distribution is repeated ten times, the distribution uniformity of the node is further improved as shown in FIG. The almost complete distribution uniformity of the node is secured in the shape space CS as shown in FIG. 7D.

Since the positions of the nodes are uniformly distributed when the step S700 is continuously performed, the difference between the node positions before and after the update is smaller than the predetermined allowable value? After a predetermined number of iterations, so that the step s600 is satisfied Step s800 is performed.

8 is a diagram showing step s800 in the robot motion path planning method of the present invention.

After the uniformity of the sufficient node distribution is ensured by the continuous repetition of the step s700, the node position is no longer largely changed as described above.

In step S800, the nodes are connected through the well-known optimal path method based on the node distribution in the finally determined shape space CS to form the optimal path of the manipulator of the robot .

As shown in FIG. 9A, when an optimal path is formed by connecting nodes having uniformity within a shape space CS, ultimately, as shown in FIG. 9B, a robot manipulator M ) Can travel without interfering with or colliding with the obstacle (WOi).

As described above, the present invention introduces the concept of the expansion restitution coefficient and the detection / repulsion coefficient by performing the steps (s200) to (s400) to secure the action of securing the uniform position of the node quickly, The path planning can be achieved in a short time.

In particular, when performing the robot motion path planning method according to the present invention, one thread is allocated to each node qi and distributedly performed. In this way, when the number of nodes (qi) increases with each thread in each node, the calculation is distributed and the operation is performed more quickly.

In addition, the neighborhood radius (r _n) for, by performing the step (s500) for updating at each operation, even if the volume of the shaped space due to a change in a dynamic environment (CS) rapidly changing due to the entry of an obstacle and the exit operation itself is not performed It does not converge even if it is calculated or operated.

The above-described embodiments are intended to illustrate the technical concept of the present invention and can not be construed as limiting the technical scope of the present invention.

Claims (6)

A method for planning a movement path of a robot under a dynamic environment,
A step (s100) of arranging a plurality of nodes qi in the shape space CS and forming a detection node? Qj at regular intervals around each of the plurality of nodes qi;
A step (s200) of comparing the radius r between the one node qi and the adjacent node qi to a predetermined neighboring radius rn to calculate an expansion / repulsion coefficient according to the following equation: s200;

Here, the node qi, the neighboring node qj, and the expansion / contraction coefficient (F ⁱ _extension ) are directional vectors.
r _ij Is the distance between the node (qi) and the node (qj)
η _i Means a set of neighboring nodes (qj).
The repulsive potential function (f) When 0 <r ≤ rn, it is determined as an arbitrary function that is inversely proportional to the distance (r), and when 0 <r ≤rn is unsatisfied, it is determined as O.
Determining whether or not the detection node around each node is located at an obstacle, and calculating the detection counterpart coefficient by the following equation (s300);

The collision detection function (s) is determined as follows
s (q) = 1 (when an obstacle collision occurs with the detection node in the shape space (CS))
= 0 (no obstacle collision occurred with the detection node in the shape space (CS))
Updating (S400) the position of the node (qi) using the following equation at each node (qi);

_Here, G _1, G ₂ are each expansion coefficient of restitution F ⁱ _extension (k) and the restitution coefficient F ⁱ index _detection gain (gain) ¹ for (k).
Determining whether the radius difference between the updated node qi (k + 1) and the updated node qi (qi) position qi (k) is equal to or smaller than a predetermined allowable value? );

Returning to step s200 if the step (s600) is not satisfied (s700);
And if the step (s700) is satisfied, terminating the robot motion path planning method. In claim 1,
Further comprising a step (s600) of updating the neighboring radius (rn) between the steps (s400) and (s600) by the following equation (s600).

G is the gain value.
Pd is a predetermined value of the sum of desired repulsive forces.
P is the sum of the repulsive forces at each node.

The time difference between kth iteration and k + 1th iteration as Δt = t _{k + 1} -t _k .
r _n (t _k ) is the neighborhood radius in the kth iteration. The method according to claim 1,
Wherein the repulsive potential function (f) is determined by the following equation.
f = 1/2 (1 / r-1 / rn) ² [0 < r &
= 0 [0 &lt; r &lt; rn satisfaction] In claim 2,
Wherein the steps (s200) to (s400) are performed in a distributed thread manner for each node. The method according to claim 1 or 2,
That is, the step (S400) of updating the position of the node (qi) does not perform the position update operation any more at the node (qi) whose position has already been updated until the position of each node (qi) And a synchronization method in which the robot moves in a waiting direction. The method according to claim 1 or 2,
Further comprising a step (s800) of connecting the nodes after the step (s600) to form an optimal path (s800).

Images