Path planning apparatus of robot and method thereof
Disclosed are a path planning apparatus of a robot, which generates a path plan to control movements of the robot, and a method thereof. An improved RRT algorithm is employed, when a tree is extended in a configuration space so as to satisfy a constraint, and thus steeply bent or roundabout portions of the tree are reduced.
1. A path planning apparatus of a robot, comprising:
a path plan generating unit configured to form a tree within a configuration space having information about an initial configuration and a final configuration, randomly sample a configuration in the configuration space, select, from a plurality of target nodes of the tree, a nearest node nearest to the sampled configuration in consideration of a distance and an angle between each of the plurality of target nodes and the sampled configuration, select a new node on a line connecting the nearest node and the sampled configuration, add the new node to the tree, and thus complete a path plan from the initial configuration to the final configuration; and
a robot control unit configured to control movements of the robot according to the path plan,
wherein the angle is an angle formed by a vector from the selected node to the sampled configuration and a vector from a parent node of the selected node to the selected node.
2. The path planning apparatus according to claim 1 , wherein the path plan generating unit selects as the nearest node a node in which the total sum of weights of functions of the distance and the angle is minimum.
3. The path planning apparatus according to claim 2 , wherein the path plan generating unit performs the selection of the nearest node by the following expression,
q
near
=
arg
min
Node
(
i
)
∈
Tree
{
W
i
Node
(
i
)
-
Sample
+
(
1
-
W
i
)
f
(
Angle
(
Node
(
i
)
,
Sample
,
Tree
)
)
}
where, Node(i) is an ith node belonging to the tree, Tree is a tree made up to the current time, Sample is the sampled configuration, |Node(i)−Sample| is a distance between the ith node and the sampled configuration, Wi is a distance weight, (1-Wi) is an angle-related weight, Angle(Node, Sample, Tree) is a function defining an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and f(Angle) is an angle-related function.
4. The path planning apparatus according to claim 1 , wherein the path plan generating unit selects as the nearest node a node in which the multiplication of functions of the distance and the angle is minimum.
5. The path planning apparatus according to claim 4 , wherein the path plan generating unit performs the selection of the nearest node by the following expression,
q
near
=
arg
min
Node
(
i
)
∈
Tree
{
Node
(
i
)
-
Sample
×
f
(
Angle
(
Node
(
i
)
,
Sample
,
Tree
)
)
}
where, Node(i) is an ith node belonging to the tree, Tree is a tree made up to the current time, Sample is the sampled configuration, |Node(i)−Sample| is a distance between the ith node and the sampled configuration, Angle(Node, Sample, Tree) is a function defining an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and f(Angle) is an angle-related function.
6. The path planning apparatus according to claim 3 , wherein the path plan generating unit defines the angle-related function f(x) by the following expression,
f ( x )=−cos( x )+ a
where, x is an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and a is a constant.
7. A path planning method of a robot comprising:
forming a tree within a configuration space having information about an initial configuration and a final configuration;
randomly sampling a configuration in the configuration space;
selecting, from a plurality of target nodes of the tree, a nearest node nearest to the sampled configuration in consideration of a distance and an angle between each of the plurality of target nodes and the sampled configuration;
selecting a new node on a line connecting the nearest node and the sampled configuration;
adding the new node to the tree to complete a path plan from the initial configuration to the final configuration; and
controlling, using a robot control unit, movements of the robot according to the path plan,
wherein the angle is an angle formed by a vector from the selected node to the sampled configuration and a vector from a parent node of the selected node to the selected node.
8. The path planning method according to claim 7 , wherein selecting the nearest node comprises selecting one of the plurality of target nodes in which the total sum of weights of functions of the distance and the angle is minimum.
9. The path planning method according to claim 7 , wherein selecting the nearest node comprises selecting one of the plurality of target nodes in which the multiplication of functions of the distance and the angle is minimum.
10. The path planning apparatus according to claim 5 , wherein the path plan generating unit defines the angle-related function f(x) by the following expression,
f ( x )=−cos( x )+ a
where, x is an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and a is a constant.
11. The path planning method according to claim 7 , wherein determining the nearest node is performed by the following expression,
q
near
=
arg
min
Node
(
i
)
∈
Tree
{
W
i
Node
(
i
)
-
Sample
+
(
1
-
W
i
)
f
(
Angle
(
Node
(
i
)
,
Sample
,
Tree
)
)
}
where, Node(i) is an ith node belonging to the tree, Tree is a tree made up to the current time, Sample is the sampled configuration, |Node(i)−Sample| is a distance between the ith node and the sampled configuration, Wi is a distance weight, (1-Wi) is an angle-related weight, Angle(Node, Sample, Tree) is a function defining an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and f(Angle) is an angle-related function.
12. The path planning method according to claim 7 , wherein determining the nearest mode is performed by the following expression,
q
near
=
arg
min
Node
(
i
)
∈
Tree
{
Node
(
i
)
-
Sample
×
f
(
Angle
(
Node
(
i
)
,
Sample
,
Tree
)
)
}
where, Node(i) is an ith node belonging to the tree, Tree is a tree made up to the current time, Sample is the sampled configuration, |Node(i)−Sample| is a distance between the ith node and the sampled configuration, Angle(Node, Sample, Tree) is a function defining an angle formed by the tree and an edge connecting the ith node and the sampled configuration, and f(Angle) is an angle-related function.