Sign up to receive free email alerts when patent applications with chosen keywords are publishedSIGN UP

Abstract:

If a manipulator of a robot falls in local minima when expanding a node to
generate a path, the manipulator may efficiently escape from local minima
by any one of a random escaping method and a goal function changing
method or a combination thereof to generate the path. When the solution
of inverse kinematics is not obtained due to local minima or when the
solution of inverse kinematics is not obtained due to an inaccurate goal
function, an optimal motion path to avoid an obstacle may be efficiently
searched for. The speed to obtain the solution may be increased and thus
the time consumed to search for the optimal motion path may be shortened.

Claims:

1. A method of planning a motion path of a robot, the method including
forming a configuration space having information about a start point, a
goal point, constraints and a goal function, expanding a tree using a
certain point selected in the configuration space and a node having a
smallest goal score in the tree, and planning the path of a manipulator
of the robot, the method comprising:determining whether local minima
occur when the tree is expanded using a processor;selecting the certain
point in the configuration space when it is determined that the local
minima occur using the processor;selecting a node closest to the certain
point in the tree using the processor; andobtaining a sample satisfying
the constraints and a predetermined goal score condition in a line
between the selected certain point and the selected node, adding the
obtained sample to the tree, and expanding the tree having the added
obtained sample, using the processor.

2. The method according to claim 1, wherein the determining of whether the
local minima occur includes determining whether a goal score of a next
node is greater than that of a previous node, and determining that the
local minima occur when a number of times of repeating determining that a
goal score of a next node is greater than that of a previous node is
equal to or greater than a predetermined number.

3. The method according to claim 2, wherein the obtaining of the sample
satisfying the predetermined goal score condition includes comparing the
goal score of the sample with the goal score of the previous node and
determining that the sample satisfies the predetermined goal score
condition when the goal score of the sample is less than that of the
previous node.

4. A method of planning a path of a robot, the method including forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator of the robot,
the method comprising:determining whether local minima occur when the
tree is expanded using a processor;changing the goal function to a new
goal function when the local minima occur, using the processor;applying
the changed goal function and selecting a node having a smallest goal
score in the tree using the processor;selecting the certain point in the
configuration space using the processor; andobtaining a sample satisfying
the constraints and a predetermined goal score condition in a line
between the selected certain point and the selected node, adding the
obtained sample to the tree, and expanding the tree having the added
obtained sample, using the processor.

5. The method according to claim 4, wherein the determining of whether the
local minima occur includes determining whether a goal score of a next
node is greater than that of a previous node, and determining that the
local minima occur when a number of times of repeating determining that a
goal score of a next node is greater than that of a previous node is
equal to or greater than a predetermined number.

6. The method according to claim 5, wherein the changing of the goal
function includes at least one of changing a directional vector having a
predetermined angle between an obstacle and the goal point to change the
goal function, and changing a directional vector having a predetermined
angle between the goal point and an end effector of the manipulator
falling in the local minima to change the goal function.

7. An apparatus to plan a motion path of a robot, the apparatus forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator of the robot,
the apparatus comprising:a path planning generator using a processor to
determine whether local minima occur when the tree is expanded, to select
the certain point in the configuration space when the local minima occur,
to select a node closest to the selected certain point in the tree, and
to obtain a sample satisfying the constraints and a predetermined goal
score condition in a line between the certain point and the selected node
to add the obtained sample to the tree such that the tree having the
added obtained sample is expanded.

8. An apparatus to plan a path of a robot, the apparatus forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator of the robot,
the apparatus comprising:a path planning generator using a processor to
determine whether local minima occur when the tree is expanded, to change
the goal function to a new goal function when the local minima occur, to
apply the changed goal function to select a node having a smallest goal
score in the tree, to select the certain point in the configuration
space, and to obtain a sample satisfying the constraints and a
predetermined goal score condition in a line between the selected certain
point and the selected node to add the obtained sample to the tree such
that the tree is expanded.

9. The apparatus according to claim 8, wherein the path planning generator
performs at least one of changing a directional vector having a
predetermined angle between an obstacle and the goal point to change the
goal function, and changing a directional vector having a predetermined
angle between the goal point and an end effector of the manipulator
falling in the local minima to change the goal function.

Description:

CROSS-REFERENCE TO RELATED APPLICATIONS

[0001]This application claims the benefit of Korean Patent Application No.
2009-0073258, filed on Aug. 10, 2009 in the Korean Intellectual Property
Office, the disclosure of which is incorporated herein by reference.

BACKGROUND

[0002]1. Field

[0003]Embodiments relate to a method and apparatus to plan a motion path
of a robot, which is not obstructed by an obstacle in a working
environment.

[0004]2. Description of the Related Art

[0005]In general, a mechanical device which performs motions similar to
human motion using an electrical or magnetic mechanism is called a robot.
Early robots included manipulators used for automated or unmanned tasks
of manufacturing plants or industrial robots, such as transportation
robots, and performed dangerous tasks, simply repeated tasks or tasks
using large force in place of a human. Recently, research into a humanoid
robot which has joints similar to those of a human, coexists with a human
in a working and living space of the human and provides various services
has been actively conducted.

[0006]Such a humanoid robot performs tasks using a manipulator which may
move similarly to the arm or hand motion of a human by an electrical or
mechanical mechanism. In most manipulators which are currently being
used, several links are connected to each other. A connection portion
between the links is called a joint. The motion characteristics of the
manipulator are determined according to a geometrical relationship.
Mathematical representation of such a geometrical relationship is called
kinematics. In general, the manipulator moves an end effector of a robot
with such kinematic characteristics in a direction (goal point) to
perform a task.

[0007]In order to allow the manipulator to perform a given task (e.g.,
grasp an object), it is important to generate a motion path from an
initial position (start point) before the manipulator performs a task to
a final position (goal point) where the task may be performed, that is,
an object may be grasped. The path in which the manipulator may move from
the start point to the goal point without colliding with an obstacle
within a working area is automatically generated by an algorithm. The
method of generating the path is divided into a process of searching for
a free space which does not cause collision with an obstacle and a
process of planning the motion path of the manipulator in the free space.

[0008]Hereinafter, in a sampling-based path planning algorithm which plans
an optimal path to connect given start and goal points while satisfying
constraints such as obstacle avoidance, a method of using a
Rapidly-exploring Random Tree (RRT) algorithm in a situation with a high
degree of freedom or complicated constraints will be described.

[0009]The RRT algorithm is a method of repeating a process of selecting a
closest node from an initial start point using a randomly sampled
configuration in a Configuration Space (C-Space) in which the manipulator
performs a task to expand a tree, and searching for a motion path to a
final goal point. In this case, a goal function is a function of a
distance from the goal point to an end-effector and a directional vector,
and a node having a smallest goal score is selected from the tree to
expand the tree.

[0010]However, since a known RRT algorithm depends on the goal score when
the closest node is selected to expand the tree reaching the final goal
point, a solution may not be obtained due to local minima if the goal
function is not accurately set.

SUMMARY

[0011]Therefore, it is an aspect of the at least one embodiment to provide
a method and apparatus to plan a path of a robot, which improves the
performance of a path plan by changing a goal function so as to
efficiently escape from local minima when falling in local minima.

[0012]Additional aspects of the at least one embodiment will be set forth
in part in the description which follows and, in part, will be obvious
from the description, or may be learned by practice of the invention.

[0013]The foregoing and/or other aspects are achieved by providing a
method of planning a path of a robot, the method including forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator, the method
including: determining whether local minima occur when the tree is
expanded using a processor; selecting a certain point when the local
minima occur using the processor; selecting a node closest to the certain
point in the tree using the processor; and obtaining a sample satisfying
the constraints and a predetermined goal score condition in a line
between the certain point and the selected node, adding the obtained
sample to the tree, and expanding the tree having the added obtained
sample using the processor.

[0014]The determining of whether the local minima occur may include
determining whether a goal score of a next node is greater than that of a
previous node, and determining that the local minima occur when a number
of times of repeating determining that a goal score of a next node is
greater than that of a previous node is equal to or greater than a
predetermined number.

[0015]The obtaining of the sample satisfying the predetermined goal score
may include comparing the goal score of the sample with the goal score of
the previous node and determining that the sample satisfies the
predetermined goal score condition if the goal score of the sample is
less than that of the previous node.

[0016]The foregoing and/or other aspects are achieved by providing a
method of planning a path of a robot, the method including forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator of the robot,
the method including: determining whether local minima occur when the
tree is expanded using a processor; changing the goal function to a new
goal function when the local minima occur using the processor; applying
the changed goal function and selecting a node having a smallest goal
score in the tree using the processor; selecting a certain point; and
obtaining a sample satisfying the constraints and a predetermined goal
score condition in a line between the certain point and the selected
node, adding the obtained sample to the tree, and expanding the tree,
using the processor.

[0017]The changing of the goal function may include at least one of
changing a directional vector having a predetermined angle between an
obstacle and a goal point so as to change the goal function, and changing
a directional vector having a predetermined angle between the goal point
and an end effector of the manipulator falling in the local minima so as
to change the goal function.

[0018]The foregoing and/or other aspects are achieved by providing an
apparatus to plan a motion path of a robot, the apparatus forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator, the apparatus
including: a path plan generator using a processor to determine whether
local minima occur when the tree is expanded, to select the certain point
when the local minima occur, to select a node closest to the certain
point in the tree, and to obtain a sample satisfying the constraints and
a predetermined goal score condition in a line between the certain point
and the selected node to add the obtained sample to the tree such that
the tree having the obtained sample added is expanded.

[0019]The foregoing and/or other aspects are achieved by providing an
apparatus to plan a path of a robot, the apparatus forming a
configuration space having information about a start point, a goal point,
constraints and a goal function, expanding a tree using a certain point
selected in the configuration space and a node having a smallest goal
score in the tree, and planning the path of a manipulator, the apparatus
including: a path plan generator using a processor to determine whether
local minima occur when the tree is expanded, to change the goal function
to a new goal function when the local minima occur, to apply the changed
goal function to select a node having a smallest goal score in the tree,
to select the certain point in the configuration space, and to obtain a
sample satisfying the constraints and a predetermined goal score
condition in a line between the certain point and the selected node to
add the obtained sample to the tree such that the tree having the
obtained sample added is expanded.

[0020]If the manipulator falls in local minima, the manipulator may
efficiently escape from local minima by any one of a random escaping
method and a goal function changing method or a combination thereof to
obtain a solution. When the solution of inverse kinematics is not
obtained due to local minima or when the solution of inverse kinematics
is not obtained due to an inaccurate goal function, an optimal motion
path to avoid an obstacle may be efficiently searched for. The velocity
to obtain the solution may be increased and thus the time consumed for
searching for the optimal motion path may be shortened.

BRIEF DESCRIPTION OF THE DRAWINGS

[0021]These and/or other aspects will become apparent and more readily
appreciated from the following description of the embodiments, taken in
conjunction with the accompanying drawings of which:

[0022]FIG. 1 is an appearance view showing an example of a robot according
to at least one embodiment;

[0023]FIG. 2 is a view showing the structure of the main joints of the
robot shown in FIG. 1;

[0024]FIGS. 3(a) and 3(b) are views schematically showing the
configuration of a kinematic redundant actuation manipulator of a robot
according to at least one embodiment;

[0025]FIG. 4 is a control block diagram of an apparatus to plan a motion
path of a manipulator in a robot according to at least one embodiment;

[0026]FIG. 5 is a view showing a Configuration Space (C-Space) according
to an embodiment;

[0027]FIG. 6 is a view showing a model of a process of grasping an object
by a robot according to at least one embodiment;

[0028]FIGS. 7 to 17 are views showing a method of generating a motion path
using a Best First Search & Rapidly Random Tree (BF-RRT) in at least one
embodiment;

[0029]FIGS. 18 to 21 are views showing various local minima situations
which may occur in a method of generating a path using an RF-RRT method
in at least one embodiment;

[0030]FIGS. 22 to 27 are views showing a method of escaping from local
minima and generating a path when falling in local minima in a method of
generating a path using an RF-RRT method in at least one embodiment;

[0031]FIGS. 28 and 29 are views explaining a concept to escape from local
minima when falling in local minima in a method of generating a path
using an RF-RRT method in at least one embodiment;

[0032]FIGS. 30 to 33 are views of a method of escaping from local minima
and generating a path when falling in local minima in a method of
generating a path using the RF-RRT method in at least one embodiment;

[0033]FIGS. 34 and 35 are views explaining in detail a concept to escape
from local minima when falling in local minima in a method of generating
a path using an RF-RRT method in at least one embodiment;

[0034]FIGS. 36 to 39 are views showing a simulation of a path plan
suggested by the at least one embodiment in a working space;

[0035]FIG. 40 is a flowchart illustrating a method of planning a motion
path of a robot according to at least one embodiment; and

[0036]FIG. 41 is a flowchart illustrating a method of planning a motion
path of a robot according to at least one embodiment.

DETAILED DESCRIPTION

[0037]Reference will now be made in detail to the at least one embodiment,
examples of which are illustrated in the accompanying drawings, wherein
like reference numerals refer to like elements throughout.

[0038]FIG. 1 is an appearance view showing an example of a robot according
to an embodiment.

[0039]In FIG. 1, the robot 100 according to the embodiment is a bipedal
robot which walks erect using two legs 110R and 110L similar to a human,
and includes a trunk 120, two arms 130R and 130L and a head 140 mounted
on an upper side of the trunk 120, and feet 111R and 111L and hands 131R
and 131L respectively mounted on the front ends of the two legs 110R and
110L and the arms 130R and 130L. In reference numerals, R and L denote
right and left, respectively.

[0040]FIG. 2 is a view showing the structure of the main joints of the
robot shown in FIG. 1.

[0041]In FIG. 2, the two arms 130R and 130L respectively include shoulder
joints 132R and 132L, elbow joints 133R and 133L and wrist joints 134R
and 134L such that the portions corresponding to the shoulders, the
elbows and the wrists of the robot 100 rotate. The shoulder joints 132R
and 132L are located on both ends of the upper side of the trunk 120.

[0042]The shoulder joints 132R and 132L of the arms 130R and 130L may move
in an x-axis (roll axis), a y-axis (pitch axis) and a z-axis (yaw axis),
the elbow joints 133R and 133L may move in the y-axis (pitch axis), and
the wrist joints 134R and 134L may move in the x-axis (roll axis), the
y-axis (pitch axis) and the z-axis (yaw axis).

[0043]The two arms 130R and 130L respectively include upper links 135R and
135L to connect the shoulder joints 132R and 132L and the elbow joints
133R and 133L and lower links 136R and 136L to connect the elbow joints
133R and 133L and the wrist joints 134R and 134L so as to move with a
predetermined degree of freedom according to the movable angle ranges of
the joints 132R and 132L, 133R and 133L, and 134R and 134L.

[0044]The trunk 120 connected to the two legs 110R and 110L includes a
waist joint 121 to rotate the waist portion of the robot 100, and the
head 140 connected to the trunk 120 includes a neck joint 141 to rotate a
neck portion of the robot 100.

[0045]In at least one embodiment, the two arms 130R and 130L configure a
manipulator 130 to perform a motional task, and the two hands 131R and
131 provided on the front end of the manipulator 130 configure an end
effector 131 to grasp a goal (object). These are shown in FIG. 3.

[0046]FIG. 3 is a view schematically showing the configuration of a
kinematic redundant actuation manipulator of a robot according to an
embodiment.

[0047]In FIG. 3, the manipulator 130 is manufactured so as to move similar
to the arm or hand motion of a human by an electrical or mechanical
mechanism. In most manipulators 130 which are currently being used,
several links (in detail, an upper link or a lower link) are connected to
each other by several joints (in detail, shoulder joints, elbow joints or
wrist joints). The motional characteristics of the manipulator 130 are
determined according to a geometrical relationship between the links and
the joints. Mathematical representation of such a geometrical
relationship is kinematics. In general, the manipulator moves the end
effector 131 with such kinematic characteristics in a direction to
perform a task. The manipulator 130 according to the embodiment moves the
end effector 131 to a goal point to grasp an object using links, the
positions and directions of which are adjusted.

[0048]As shown in FIG. 3, the shape of the manipulator 130 which moves to
the goal point in order to grasp the same object may be variously changed
as shown in FIG. 3(a) or 3(b).

[0049]FIG. 4 is a control block diagram of an apparatus to plan a motion
path of a manipulator in a robot according to an embodiment, which
includes a user interface unit 200, a path planning generator 210, a
recognizer 220, a robot controller 230, and a driving unit 240.

[0050]The user interface unit 200 allows a user to input a task command
(e.g., a grasping command to grasp an object placed on a table) of the
robot and, more particularly, the manipulator 130 by manipulating a
switch or via voice, for example, but is not limited to these examples
and may be any type of input device.

[0051]The path planning generator 210 generates and sends a path plan to
control the movement of the manipulator 130 according to the task command
input via the user interface unit 200 to the robot controller 230. A
method of generating a path plan by the path plan generator 210 includes
a space forming operation, a graph searching operation and a path
generating operation.

[0052]In the space forming operation, a Configuration Space (C-Space) to
generate a collision avoidance path is detected. The term "configuration"
refers to a set of variables to represent the position and the direction
of the moving manipulator 130, and all spaces occupied by configurations
are called the C-Space.

[0053]In the graph searching operation, a connected network representing a
searchable path to generate an optimal path is generated. Configurations
are randomly extracted based on the C-Space formed by a cell
decomposition method such as a tree with respect to the moving
manipulator 130, nodes are generated by a method of excluding a
configuration colliding with an obstacle space, and a search tree to
connect the generated nodes is generated, thereby searching for a search
graph to generate a path.

[0054]In the path generation operation, an obstacle space is avoided so as
not to cause collision in the connected network of the given search
space, and an optimal path connected between a start point and a goal
point is generated.

[0055]The recognizer 220 recognizes information which is given in order to
perform a task command, that is, a configuration (start point) at an
initial position of the manipulator 130 before performing the task
command, a configuration (goal point) at a goal point of the manipulator
130 where the task command may be performed, and obstacles between the
start point and the goal point, in the C-Space, and sends the information
to the path planning generator 210. This recognized information is used
to plan the motion path of the manipulator 130 by the path planning
generator 210.

[0056]The robot controller 230 controls the driving unit 240 according to
the path plan received from the path planning generator 210, drives the
manipulator 130, and controls the movement of the manipulator 130.

[0057]Hereinafter, the robot having the above-described structure, the
method of planning the path thereof, and the effects thereof will be
described.

[0059]In FIG. 5, the C-Space refers to a dynamic space where the
manipulator 130 performs a task, S denotes a start point in which the
configuration at the initial position before performing the task is
formed by one node in the C-Space, G denotes a goal point in which the
configuration at the goal position where the task may be performed, that
is, an object, may be grasped, is formed by one node in the C-Space, and
K denotes an obstacle in the C-Space between the start point S and the
goal point G.

[0060]An example of a method of planning a motion path, while satisfying
constraints, such as obstacle collision avoidance, includes a sampling
based motion planning method.

[0061]As such a method, there is a Rapidly Random Tree (RRT) searching
method.

[0062]A tree expansion method of the RRT is largely divided into a
Single-RRT, a Bi-Directional RRT (BI-RRT), and a Best First Search & RRT
(BF-RRT). The Single-RRT is used to travel from the initial configuration
to the goal configuration. One configuration is randomly selected in the
C-Space with the initial configuration, the goal configuration and the
constraints, a node closest to the randomly selected configuration is
selected, a virtual line is drawn, a sample in the virtual line is
included if the sample satisfies the constraints, and the tree is
continuously expanded until the sample becomes equal to the goal
configuration.

[0063]In the Single-RRT, since a certain point is selected and the tree is
expanded in order to reach the goal configuration, a relatively large
number of nodes are generated and thus a path search speed is decreased.
In order to prevent the path search speed from being decreased, in the
BI-RRT, the tree is expanded not only in the start configuration but also
in the goal configuration (while swapping the tree). If the tree is
connected while the tree is expanded, the graph searching algorithm is
performed so as to obtain a path.

[0064]Both the Single-RRT and the BI-RRT are characterized in that the
goal configuration is set. However, in the case of a robot with a
redundant degree of freedom, there may be various configurations reaching
the goal point (see FIG. 3).

[0065]In addition, if a configuration reaching the goal point is a
configuration in which collision with an obstacle occurs midway, a path
reaching the goal configuration may not be generated.

[0066]The BF-RRT refers to a method of satisfying the goal score of the
goal point instead of the configuration of the goal point. In this
method, inverse kinematics is not initially solved and a sample is
expanded so as to reach a final position in the case of a redundant
manipulator. The redundant manipulator refers to a robot arm having a
higher degree of freedom than that necessary to perform a task in a
working space.

[0067]As shown in FIG. 6, a goal function which is satisfied when a robot
grasps a goal object includes a distance ∥G-H∥ between
a goal point G and the end effector of the robot and a directional vector
|(G-H)*xH-1|.

Goal Function=∥G-H∥+α*|(G-H)*xH-1| Equation
1

[0068]where, ∥G-H∥ denotes the distance from the end
effector to the goal point, |(G-H)*xH-1| denotes the directional
vector, and a denotes a constant.

[0069]The BF-RRT performs the following operations.

[0070]Operation 0: A C-Space with a goal point S, a goal point G,
constraints, and a goal function is formed. The start point S, the goal
point G and the constraints to avoid collision with an obstacle K are
recognized. In addition, the goal function, which is the function of the
distance from the goal point G to the end effector, and the directional
vector are recognized. In addition, the C-Space is formed from the start
point S, the goal point G, the constraints and the goal function (see
FIG. 7).

[0071]Operation 1: A certain point is randomly sampled and selected in the
C-Space.

[0072]Operation 2: A node having a smallest goal score is selected in the
tree. In an initial state, the node having the smallest goal score in the
tree is the start point S.

[0073]Operation 3: The selected node and the certain point are connected.
At this time, a new sample is stored only when the goal function is less
than a previous goal function.

[0074]As shown in FIGS. 8 and 9, a certain point Q is sampled (see FIG.
8). A virtual line (dotted line) is connected between the sampled point Q
and a node Q-new having a smallest goal score, and a new sample separated
from the sampled point by a predetermined distance is then generated (see
FIG. 9). Constraints (joint limit, collision avoidance, etc.) and a goal
score of the new sample are checked and the sample is added if the
constraints and the goal score condition are satisfied (see FIGS. 10 and
11). Such a method is repeatedly performed until a new sample is
constantly expanded, such that a tree T1 is expanded to a tree T2.

[0075]FIGS. 12 to 15 show operations to repeatedly perform Operations 1, 2
and 3 so as to add new samples to the tree.

[0076]Operation 4: If the expanded node satisfies the goal score
condition, it is determined that a final node satisfies the goal function
so as to generate a path by graph searching (see FIGS. 16 and 17).

[0077]The BF-RRT is advantageous in that a solution may be obtained even
when the solution of the inverse kinematics is not present or the
solution of the inverse kinematics is present in a path including an
obstacle. However, since a point having a smallest goal score is selected
so as to expand the tree, local minima may occur if the goal function is
not adequately set.

[0078]FIGS. 18 to 21 are views showing various local minima which occur
while a tree is expanded.

[0079]If it is determined that local minima occur while Operations 1 to 3
of the BF-RRT are performed, the Single-RRT method is partially applied.
By applying this method, the solution may be obtained when the solution
of the inverse kinematics is not obtained, when local minima occur or
when the directional vector of the goal function is not accurate.

[0080]In more detail, if local minima occur while Operations 1 to 3 of the
existing BF-RRT are repeatedly performed,

[0081]Operation 4: It is determined whether local minima occur.

[0082]Operation 5: If it is determined that local minima occur, the
existing tree is expanded in a certain direction.

[0083]Operation 6: A closest node is selected and is connected to the
tree.

[0084]As shown in FIG. 22, if local minima occur while the tree is
expanded, a certain point is randomly selected and is then connected to a
closest node, as shown in FIG. 23.

[0085]As shown in FIGS. 24 to 27, if local minima continuously occur, the
tree is expanded from the existing tree in a certain direction, a closest
node is selected and the tree is connected again. If local minima do not
occur, Operations 1, 2 and 3 are performed, a certain point is randomly
sampled and selected in the C-Space, a node having a smallest goal score
in the tree is selected, and the selected node and the certain point are
connected. If the goal condition is satisfied while Operations 1, 2 and 3
are repeated, the final node is recognized as the goal point and a path
is generated by graph searching.

[0086]In the above-described method, the search space is increased as in
the Single-RRT due to the characteristics in which a certain point is
randomly selected so as to expand the tree when local minima occur as
shown in FIGS. 24 to 27.

[0087]Therefore, time consumed to obtain a final solution may be
increased.

[0088]In contrast, if the goal function is changed and new directional
vectors are applied when local minima occur, the solution of the inverse
kinematics may be more efficiently obtained.

[0089]In more detail, while Operations 1, 2 and 3 of the BF-RRT are
performed,

[0090]Operation 4: It is determined whether local minima occur.

[0091]Operation 5: If local minima occur, a new directional vector having
a predetermined angle between the goal point and an intermediate obstacle
or between the goal point and a node falling in local minima is selected
instead of the current approach direction, and a goal score is set again.

[0092]Operation 6: A point closest to a goal determined by the new goal
score and a certain point are connected.

[0093]Operation 5-1: If local minima occur again, the directional vector
is changed.

[0094]Operation 5-2: If there is no new directional vector, a certain
directional vector is selected.

[0095]Operation 7: If a distance from the goal point is equal to or less
than a predetermined distance, the goal score is set again using the
directional vector of the existing end effector.

[0096]Operation 8: If the goal score satisfies the goal condition during
expansion, the final node is recognized as the goal point and the graph
searching algorithm is performed, thereby searching for a path.

[0097]As shown in FIG. 28, if local minima occurs while expansion is
performed using the existing goal function, the existing goal vector is
changed to a new goal vector.

[0098]By applying the new goal vector, a new node Q-new having a smallest
goal score is selected and is connected to an arbitrarily selected point
Q (see FIG. 29).

[0100]Subsequently, new nodes having small goal scores are selected (see
FIG. 31).

[0101]If a node is present within a predetermined distance from the goal
point, the changed goal vector is changed to the goal vector prior to
change and is connected (see FIG. 32).

[0102]If the goal score converges to a predetermined value, the final node
is recognized as the goal point and the path is searched for by graph
searching (see FIG. 33).

[0103]Hereinafter, a method of changing the goal vector will be described.

[0104]There are various methods of setting the goal vector. Among them, a
method of selecting a vector having a predetermined angle between an
obstacle and a goal point (see FIG. 34) and a method of selecting a
vector having a predetermined angle between an end effector falling in
local minima and a goal point (see FIG. 35) will be described.

[0105]Both the above-described methods are stored. Next, if the first
method fails, the second method may be selected.

[0106]The goal function is as follows.

Goal Function=∥G-H∥+α|(G-H)xH-1|

[0107]where, ∥G-H∥ denotes the distance from the end
effector to the goal point, |(G-H)*xH-1| denotes the directional
vector, and a denotes a constant.

[0108]Among them, the portion "G-H" is called a goal vector.

[0109]The newly defined goal vector and goal function are determined by
the following process.

[0110]First, as shown in FIG. 34, in a method using an obstacle and a goal
point, a new goal function is determined by the following equation.

New Goal Function=∥G-H∥+α|A2xH-1|

A1=Normalize (goal point-Obs)θ1=a cos(A1A2)

[0111]where, |A2*xH-1| denotes the newly defined directional vector,
∥G-H∥ denotes the distance from the end effector to the
goal point, and a denotes a constant.

[0112]A1 denotes the directional vector between the center Obs of the
obstacle and the goal point.

[0113]A2 denotes a new directional vector satisfying the above-described
condition. Using this vector, the new goal function is set.

[0114]Meanwhile, as shown in FIG. 35, in a method using an end effector
falling in local minima and a goal point, a new goal function is
determined by the following equation.

New Goal Function=∥G-H∥+α*|B2*xH-1|

B1=Normalize (goal point-EE)θ2=a cos(B1B2)

[0115]where, |B2*xH-1| denotes the newly defined directional vector,
∥G-H∥ denotes the distance from the end effector to the
goal point, and a denotes a constant.

[0116]B1 denotes the directional vector between the center Obs of the
obstacle and the end effector.

[0117]B2 denotes a new directional vector satisfying the above-described
condition. Using this vector, the new goal function is set.

[0118]At this time, if the end effector does not escape from local minima
even when the changed directional vector is used, a random direction is
selected or a closest point is selected so as to perform expansion.

[0119]FIG. 36 shows an initial condition and the shape of an obstacle K
when the manipulator 130 grasps a ball which is an object C placed on a
table while avoiding collision with a cylinder which is an obstacle.

[0120]FIG. 37 shows the case where a tree representing a motion path is
not expanded due to local minima when the manipulator 130 grasps a ball.

[0121]FIG. 38 shows the case where the manipulator 130 escapes from local
minima through random escape such that the solution of inverse kinematics
is obtained. In this case, the number of nodes generated is great and the
motion path to avoid the cylinder is generated to become distant from the
ball. Accordingly, a speed to obtain the solution of inverse kinematics
is decreased and thus a time consumed to form the motion path is
increased.

[0122]FIG. 39 shows the case where the manipulator 130 escapes from local
minima by changing the goal function such that the solution of inverse
kinematics is obtained. In this case, the number of nodes generated is
small and the motion path to avoid the cylinder is generated so as to
become close to the ball, compared with FIG. 38. Accordingly, a speed to
obtain the solution of inverse kinematics is increased and thus a time
consumed to form the motion path is decreased.

[0123]As described above, the solution of the inverse kinematics may be
rapidly obtained by random searching or direction change when the
solution of inverse kinematics is not obtained due to local minima or due
to an inaccurate goal function.

[0124]FIG. 40 is a flowchart illustrating a method of planning a path of a
robot according to an embodiment.

[0125]Referring to FIG. 40, the path planning generator 210 recognizes
information given when the manipulator 130 performs the task command,
that is, the configuration (start point S) at the initial position of the
manipulator 130 before performing the task command, the configuration
(goal point G) at the goal position of the manipulator 130 where the task
command may be performed, and the constraints to avoid the obstacle
between the start point S and the goal point G, in the C-Space using the
recognizer 220 (300).

[0126]The path planning generator 210 sets the goal function which is the
function according to the distance and direction difference between the
start point S and the goal point G (301).

[0127]After setting the goal function, the path planning generator 210
selects a certain point which is randomly selected when the tree is
generated in order to plan the path satisfying the constraints in the
C-Space (302).

[0128]A node having a smallest goal score according to the goal function
is obtained from the existing tree (303).

[0129]A new sample is obtained on a line connected between the selected
certain point and the node having the smallest goal score in the existing
tree (304).

[0130]In addition, the goal score of the new sample and the goal score of
the node having the smallest goal score in the existing tree are compared
to determine whether the goal score of the new sample is less than that
of the node having the smallest goal score in the existing tree (305).

[0131]If it is determined that the goal score of the new sample is less
than that of the node having the smallest goal score in the existing tree
in Operation 305, the new sample is added to the tree (306).

[0132]In contrast, if it is determined that the goal score of the new
sample is not less than that of the node having the smallest goal score
in the existing tree in Operation 305, the new sample is discarded (307).

[0133]It is determined if the number of times of repeating the
determination that the goal score of the new sample is not less than that
of the node having the smallest goal score in the existing tree is equal
to or greater than a first predetermined local minimum number (308). If
the number of times of repetition is equal to or greater than the first
predetermined local minimum number, it is determined that local minima
occur and thus a certain point is randomly selected (309) and a node
closest to the selected point in the tree is obtained as a new node on a
line connected between the new node and the selected point (310). A new
sample is obtained (311). The new sample is added to the tree (306).

[0134]In order to determine whether the new sample reaches the goal point
after the new sample is added to the tree, the goal score of the new
sample and the goal score of the goal point are compared to determine
whether the goal score of the new sample is less than or equal to the
goal point (312).

[0135]If it is determined that the goal score of the new sample is less
than or equal to the goal point in Operation 312, it is determined that
the new sample reaches the goal point and the motion path of the
manipulator 130 is generated from several nodes on the tree (313).

[0136]Meanwhile, if the goal score of the new sample is greater than that
of the goal point in Operation 312, it is determined that the new sample
does not reach the goal point and determines whether the number of times
of repeating the determination that the goal score of the new sample is
greater than that of the goal point is equal to or greater than a
predetermined maximum number (314). If it is determined that the number
of times of repetition is equal to or greater than the predetermined
maximum number in Operation 314, a warning to indicate that the
generation of the path of the manipulator 130 of the robot fails is
provided to the user (315). In contrast, if it is determined that the
number of times of repetition is less than the predetermined maximum
number in Operation 314, the method progresses to Operation 308 and
determines whether the number of times of repetition is equal to or
greater than the first predetermined local minimum number (308). If it is
determined that the number of times of repetition is equal to or greater
than the first predetermined local minimum number in Operation 308, it is
determined that local minima occur. Then, in order to randomly select a
certain point, the method progresses to Operation 309. Meanwhile, if it
is determined that the number of times of repetition is less than the
first predetermined local minimum number in Operation 308, it is
determined whether the number of times of repetition is equal to or
greater than a second predetermined local minimum number set to be less
than the first predetermined local minimum number (316). If it is
determined that the number of times of repetition is less than the second
predetermined local minimum number in Operation 316, the method
progresses to Operation 302 and the subsequent operations are performed.
In contrast, if it is determined that the number of times of repetition
is equal to or greater than the second predetermined local minimum number
in Operation 316, the existing preceding node q_front is discarded and
the preceding node q_front is replaced with the parent node of the
preceding node q_front (317). Then, the method progresses to Operation
302 and the subsequent operations are performed.

[0137]After the motion path of the manipulator 130 of the robot is formed
by the above-described method and the motion path of the manipulator 130
is searched for by the graph searching operation to generate a path plan,
the robot controller 230 controls the driving unit 240 according to the
generated path plan so as to control the motion of the manipulator 130.

[0138]FIG. 41 is a flowchart illustrating a method of planning a path of a
robot according to an embodiment.

[0139]Referring to FIG. 41, the path planning generator 210 recognizes
information given when the manipulator 130 performs the task command,
that is, the configuration (start point S) at the initial position of the
manipulator 130 before performing the task command, the configuration
(goal point G) at the goal position of the manipulator 130 where the task
command may be performed, and the constraints to avoid the obstacle
between the start point S and the goal point G, in the C-Space using the
recognizer 220 (400).

[0140]The path planning generator 210 sets the goal function which is the
function according to the distance and direction difference between the
start point S and the goal point G (401).

[0141]After setting the goal function, the path plan generator 210 selects
a certain point which is randomly selected when the tree is generated in
order to plan the path satisfying the constraints in the C-Space (402).

[0142]A node having a smallest goal score according to the goal function
is obtained from the existing tree (403).

[0143]A new sample is obtained on a line connected between the selected
certain point and the node having the smallest goal score in the existing
tree (404).

[0144]In addition, the goal score of the new sample and the goal score of
the node having the smallest goal score in the existing tree are compared
to determine whether the goal score of the new sample is less than that
of the node having the smallest goal score in the existing tree (405).

[0145]If it is determined that the goal score of the new sample is less
than that of the node having the smallest goal score in the existing tree
in Operation 405, the new sample is added to the tree (406).

[0146]In contrast, if it is determined that the goal score of the new
sample is not less than that of the node having the smallest goal score
in the existing tree in Operation 405, the new sample is discarded (409).

[0147]It is determined whether the number of times of repeating the
determination that the goal score of the new sample is not less than that
of the node having the smallest goal score in the existing tree is equal
to or greater than a first predetermined local minimum number (410). If
the number of times of repetition is equal to or greater than the first
predetermined local minimum number, it is determined that local minima
occur and thus a certain point is randomly selected (413). Then, the
current goal function is changed to a predetermined new goal function
and, when the changed goal function is applied, a node having a smallest
goal score is obtained from the existing tree by the changed function as
a new node (414). A new sample is obtained on a line connected between
the new node and the selected point (415). Then, the goal score of the
new sample and the goal score of the node having the smallest goal score
in the existing tree are compared to determine whether the goal score of
the new sample is less than that of the node having the smallest goal
score in the existing tree (405).

[0148]If it is determined that the goal score of the new sample is less
than the goal score of the node having the smallest goal score in the
tree in Operation 405, the new sample is added to the tree (406).

[0149]In order to determine whether the new sample reaches the goal point
after the new sample is added to the tree, the goal score of the new
sample and the goal score of the goal point are compared so as to
determine whether the goal score of the new sample is less than or equal
to that of the goal point (407). If it is determined that the goal score
of the new sample is less than or equal to the goal score of the goal
point in Operation 407, it is determined that the new sample reaches the
goal point and the motion path of the manipulator 130 is generated from
several nodes on the tree (408).

[0150]Meanwhile, if the goal score of the new sample is greater than that
of the goal point in Operation 407, it is determined whether the added
new sample is within a predetermined distance from the goal point (416).
If the added new sample is within the predetermined distance from the
goal point, the method progresses to Operation 414 of changing the goal
function to an original goal function and the subsequent operations are
then performed.

[0151]In contrast, if the added new sample is not within the predetermined
distance from the goal point, it is determined whether the number of
times of repeating the determination that the goal score of the new
sample is greater than that of the goal point is equal to or greater than
a predetermined maximum number (417). If it is determined that the number
of times of repetition is equal to or greater than the predetermined
maximum number in Operation 417, a warning in which the generation of the
path of the manipulator 130 of the robot fails is given to the user (418)

[0152]In contrast, if it is determined that the number of times of
repetition is less than the predetermined maximum number in Operation
417, the method progresses to Operation 410 of determining whether the
number of times of repetition is equal to or greater than a first
predetermined local minimum number (410). If it is determined that the
number of times of repetition is equal to or greater than the first
predetermined local minimum number in Operation 410, it is determined
that local minima occur. Then, in order to randomly select a certain
point, the method progresses to Operation 413.

[0153]Meanwhile, if it is determined that the number of times of
repetition is less than the first predetermined local minimum number in
Operation 410, it is determined whether the number of times of repetition
is equal to or greater than a second predetermined local minimum number
set to be less than the first predetermined local minimum number (411).
If it is determined that the number of times of repetition is less than
the second predetermined local minimum number in Operation 411, the
method progresses to Operation 402 and the subsequent operations are
performed.

[0154]In contrast, if it is determined that the number of times of
repetition is equal to or greater than the second predetermined local
minimum number in Operation 411, the existing preceding node Q-front is
discarded and the preceding node Q-front is replaced with the parent node
of the preceding node Q-front (412). Then, the method progresses to
Operation 402 and the subsequent operations are performed.

[0155]After the motion path of the manipulator 130 of the robot is formed
by the above-described method and the motion path of the manipulator 130
is searched for by the graph searching operation to generate a path plan,
the robot controller 230 controls the driving unit 240 according to the
generated path plan to control the motion of the manipulator 130.

[0156]One or more of units/hardware described in the application may also
be configured to act as one or more software modules in order to perform
the operations of the above-described embodiments and may cause a general
purpose computer or processor or a particular machine to perform any of
the operations discussed above.

[0157]Aspects of the present at least one embodiment can also be embodied
as computer-readable codes on a computer-readable medium. Also, codes and
code segments to accomplish the present at least one embodiment can be
easily construed by programmers skilled in the art to which the present
invention pertains. The computer-readable medium may be a
computer-readable storage medium, which is any data storage device that
can store data which can be thereafter read by a computer system or
computer code processing apparatus. Examples of the computer-readable
storage medium include read-only memory (ROM), random-access memory
(RAM), CD-ROMs, magnetic tapes, floppy disks, and optical data storage
devices. The computer-readable medium can also be distributed over
network-coupled computer systems so that the computer-readable code is
stored and executed in a distributed fashion.

[0158]Although a few embodiments have been shown and described, it would
be appreciated by those skilled in the art that changes may be made in
these embodiments without departing from the principles and spirit of the
invention, the scope of which is defined in the claims and their
equivalents.