VO
v
A
S
S
Sf
d
A r
Figure 14: a: TG strategy. b: MV strategy. c: ST strategy.
4.2. Heuristic Search
For on-line applications, the trajectory can be generated incrementally by expanding
only the node corresponding to the current robot position, and generating only one branch
per node, using some heuristics to choose among all possible branches.
The heuristics can be designed to satisfy a prioritized series of goals, such as survival of the robot as the rst goal, and reaching the desired target, minimizing some
performance index, and selecting a desired trajectory structure, as the secondary goals.
Choosing velocities in RAV (if they exist) automatically guarantees survival. Among
those velocities, selecting the ones along the straight line to the goal would ensure reaching the goal. Selecting the highest feasible velocity in the general direction of the goal
may reduce motion time. Selecting the velocity from the appropriate subset of RAV can
ensure a desired trajectory structure (front or rear maneuvers). It is important to note
that there is no guarantee that any objective is achievable at any time. The purpose of
the heuristic search is to nd a "good" local solution if one exist.
We have experimented with the following basic heuristics:
1. Choose the highest avoidance velocity along the line to the goal, as shown in Figure 14-a, so that the trajectory takes the robot towards its target. This strategy is
denoted in the following by TG (to goal).
2. Select the maximum avoidance velocity within some specied angle  from the line
to the goal, as shown in Figure 14-b, so that the robot moves at high speed, even if it
does not aim directly at the goal. This strategy is henceforth called MV (maximum
velocity).
3. Select the velocity that avoids the obstacles according to their perceived risk. This
strategy is called ST (structure). In Figure 14-c the chosen velocity is the highest
among the rear avoidance velocities.
Other heuristics may combine some or all of the above strategies. For example, the
avoidance velocity could be chosen as the fastest velocity pointing towards the target
and generating a rear avoidance maneuver. It might be advantageous to switch between
heuristics in order to yield trajectories that better satisfy the prioritized goals. Examples
of trajectories generated by these heuristics are presented in the following Section.
15
target
1
2


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g017.jpg
Figure 17
Obstacle avoidance and local oscillation of manipulator. (a) Obstacle avoidance of manipulator; (b) Obstacle-avoidance local oscillation of manipulator.


B
B
^
V
B
V
B
VOB
VOB
B
^
1
1
1
1
2
2
2
2
Figure 4: Velocity obstacles for B1 and B2.
where  is the Minkowski vector sum operator.
The VO partitions the absolute velocities of A into avoiding and col liding velocities.
Selecting vA outside of V O would avoid collision with B, or:
A(t) \ B(t) = ; if vA(t) 62 V O(t) (3)
Velocities on the boundaries of V O would result in A grazing B. Note that the VO of a
stationary obstacle is identical to its relative velocity cone, since then vB = 0.
To avoid multiple obstacles, we consider the union of the individual velocity obstacles:
V O = [m
i=1V OBi (4)
where m is the number of obstacles. The avoidance velocities, then, consist of those
velocities vA, that are outside all the V O's, as shown in Figure 4.
In the case of many obstacles, it may be useful to prioritize the obstacles so that
those with imminent collision will take precedence over those with long time to collision.
Furthermore, since the VO is based on a linear approximation of the obstacle's trajectory,
using it to predict remote collisions may be inaccurate, if the obstacle does not move
along a straight line. We call imminent a collision between the robot and an obstacle if it
occurs at some time t < Th, where Th is a suitable time horizon, selected based on system
dynamics, obstacle trajectories, and the computation rate of the avoidance maneuvers.
To account for imminent collisions, we modify the set V O by subtracting from it the
set V Oh dened as:


V
B
A
^
B
^
VOB
V
B
B
^
λB f
B r
B r
λ
B f λ
λ
Sff
S
fr S
rr
S
rf
V
A
1 1
1 1
2
1
2
2 2
2
Figure 12: Classication of the reachable avoidance velocities.
The properties of the RAV generated by multiple obstacles are summarized in the
following Theorem, whose proof is given in Appendix A.
Theorem 1: Given a robot A and m moving obstacles Bj (j = 1; : : : ; m), the reachable
avoidance set RAV consists of at most 3 m subsets, each including velocities corresponding
to a unique type of avoidance maneuver. 2
The Theorem states that it is possible to subdivide the avoidance velocities RAV into
subsets, each corresponding to a specic avoidance maneuver of an obstacle. For example,


Safe Interval Path Planning with Kinodynamic Constraints
Zain Alabedeen Ali1
, Konstatin Yakovlev2, 3
1 Moscow Institute of Physics and Technology, Moscow, Russia
2 Federal Research Center for Computer Science and Control of Russian Academy of Sciences, Moscow, Russia
3 AIRI, Moscow, Russia
ali.za@phystech.edu, yakovlev@isa.ru
Abstract
Safe Interval Path Planning (SIPP) is a powerful algorithm for
solving a single-agent pathfnding problem where the agent
is confned to a graph and certain vertices/edges of this graph
are blocked at certain time intervals due to dynamic obstacles
that populate the environment. The original SIPP algorithm
relies on the assumption that the agent is able to stop instantaneously. However, this assumption often does not hold
in practice, e.g. a mobile robot moving at a cruising speed
cannot stop immediately but rather requires gradual deceleration to a full stop that takes time. In other words, the robot
is subject to kinodynamic constraints. Unfortunately, as we
show in this work, in such a case, the original SIPP is incomplete. To this end, we introduce a novel variant of SIPP that
is provably complete and optimal for planning with acceleration/deceleration. In the experimental evaluation, we show
that the key property of the original SIPP still holds for the
modifed version: it performs much fewer expansions compared to A* and, as a result, is notably faster.
Introduction
Planning a path in the presence of both static and moving
obstacles is a challenging problem with topical applications
in robotics, video games, and other domains. When the environment is fully observable and the trajectories of the dynamic obstacles are known (e.g., are predicted by the robot’s
perception system or by a global observer), it is reasonable
to account for them while planning. A prominent method
that is tailored for such a setting is Safe Interval Path Planning (SIPP) (Phillips and Likhachev 2011). This is a searchbased algorithm that operates on a graph where vertices correspond to the confgurations of the agent (e.g., position,
heading, velocity, etc.) and edges correspond to the transitions between them. Due to the dynamic obstacles, certain
vertices/edges of this graph are blocked at certain time intervals. SIPP accounts for that and constructs plans in which an
agent can wait at the vertices to avoid collisions. An explicit
assumption of SIPP is that the agent can start/stop moving instantaneously. Under this assumption, the algorithm is
provably complete and optimal (w.r.t. the given spatial and
temporal discretization).
Copyright © 2023, Association for the Advancement of Artifcial
Intelligence (www.aaai.org). All rights reserved.
A
B
C
t0 = 6


Go to:
Funding Statement
This research was funded by the National Natural Science Foundation of China (51105213) and the Demonstration and Guidance Special Project of Science and Technology for the Benefit of the People in Qingdao (22-3-7-smjk-11-nsh).


Figure 4. The velocity potential fields. (a) Static obstacle. (b) Moving obstacle.
RVF
 is formed around obstacles for avoiding collision when the robot approaches the obstacle to a certain distance, which is claimed as a radius of repulsive potential field dr, the smaller the distance is, the greater the repulsive velocity is,
 and
 are the nearest two points between the robot and the obstacle, and then the direction of
 is from O to R.
A safe area of sphere O with a radius ds is set. OP is a circle-shaped cross section of the sphere O in the plane P that is formed by
 and

 as shown in Figure 5.

 is the equivalent speed at point R to

 at point N. Tangential velocity function (TVF)
 is used to bypass obstacles to avoid the local minima problem. To make sure the path of collision avoidance in 3-D space as short as possible,
 should be tangent to the circle OP. η is an angle from OR clockwise rotation to OG in the plane P.


VB
VOB
V
A
VB
A
^
RAV
K
H
L
M
Figure 7: The reachable avoidance velocities RAV.
of feasible accelerations at time t, FA(t), is dened as:
FA(t) = fx j x = f (x; x_ ; u); u 2 Ug (6)
where x is the position vector, f (x; x_ ; u) represents the robot dynamics, u is the vector
of the actuator eorts, and U is the set of admissible controls.
The set of reachable velocities, RV(t + t), over the time interval t, is thus dened
as:
RV(t + t) = fv j v = vA(t)  t  FA(t)g (7)
It is computed by scaling FA(t) by t and adding it to the current velocity of A, as shown
schematically in Figure 6.
The set of reachable avoidance velocities, RAV, is dened as the dierence between
the reachable velocities and the velocity obstacle:
RAV(t + t) = RV(t + t) 	 V O(t) (8)
where 	 denotes the operation of set dierence. A maneuver avoiding obstacle B can
then be computed by selecting any velocity in RAV. Figure 7 shows schematically the
set RAV consisting of two disjoint closed subsets. For multiple obstacles, the RAV may
consist of multiple disjoint subsets.
It is important to note that the reachable set (7) accounts only for constraints on the
robot accelerations, which we call dynamic constraints. In the context of mobile robots,
non-holonomic kinematic constraints, which prevent the vehicle from moving sideways,
need also be considered. However, the non-holonomic constraints are relevant only at low
speeds, such as during parking, since at high speeds the dynamic constraints are generally more restrictive, as shown schematically in Figure 8. In other words, the bounded
9
A
^
VA
due to dynamic
constraints


Method
As our method relies on SIPP, which in turn relies on A*
with timesteps, we frst discuss the background and then
delve into the details of the suggested approach. We assume
that a reader is familiar with vanilla A* for static graphs.
Background
A* with time steps The straightforward way to solve the
considered problem is to use A* for searching the state
space whose nodes are the tuples n = (v, t), where v is
1
Please note that our problem formulation, as well as the suggested method, is agnostic to how the state variables, e.g. velocity,
change while the agent is executing a motion primitive. We are interested in the values of the state variables only at the endpoints of
a motion primitive.
the graph vertex and t is the time step by which it is reached.
When expanding a node, the successors are generated as follows. First, for each e = (v, u), if e is feasible, the node
nmove = (u, t + w(e)) is added to the successors. Indeed,
if the application of a motion e at time step t results in a
collision, the resulting node is discarded. Second, if v allows for the wait, then the node corresponding to the wait
action of the minimal possible duration of one time step
nwait = (v, t + 1) is also added to successors. Again, if
the safe intervals of v do not cover the time step t + 1, this
node is discarded.
Other parts of the search algorithm are exactly the same
as in A*. Notably, the g-values of the nodes equal the time
components of their identifers. I.e., the g-value of the node
n = (v, t) equals t, as this is the minimal known estimate of
the cost (w.r.t to the current iteration of the algorithm) form
start to v.
SIPP In many scenarios, A* with time steps generates numerous nodes of the form (v, t),(v, t + 1),(v, t + 2), etc.,
which are created via the use of the atomic wait actions. This
leads to the growth of the search tree and slows down the
search. To this end, SIPP relies on the idea of compressing
the sequential wait actions to reduce the number of the considered search nodes.
The search nodes of SIPP are identifed as n =
(v, [lbj , ubj ]), where [lbj , ubj ] is the j-th safe interval of
v. For each node, SIPP stores the earliest arrival time:
t ∈ [lbj , ubj ]. Whenever a lower-cost path to n is found,
t(n) is updated, and this value serves as the g-value of the
node similarly to A* with time steps.
When expanding a node, SIPP does not generate any separate successor corresponding to the wait action. Instead, it


∗e-mail:kourosh.naderi@aalto.fi
†
e-mail:joose.rajamaki@aalto.fi
‡
e-mail:perttu.hamalainen@aalto.fi
1 Introduction
Path planning in dynamic environments is a demanding problem
encountered in many robotic tasks and computer games [Rastgoo
et al. 2014; Sud et al. 2008]. Real-time path planning algorithms
are used to react to the changes in the environment as well as to
constantly look for a better path to the goal point. These algorithms
have to balance with the trade-off of the goodness of the path versus
having a short search time. The path planning problem gets even
more challenging when changing the goal point is allowed, which
is the case in many multiple-query tasks [Kavraki et al. 1996], e.g.
computer games. Difficulty in path planning arises also when the
structure of the agent gets more sophisticated and controlling the
agent is not trivial [Sud et al. 2008], e.g. planning a path for a
humanoid agent [Gutmann et al. 2005].
Many of the current approaches for solving the real-time path planning problem fall into the categories of heuristic methods [Gutmann et al. 2005], potential field methods [Khatib 1986] and sampling methods like rapidly exploring random trees (RRTs) [LaValle
1998]. A survey of approaches to real-time path planning in dynamic environment can be found in [Rastgoo et al. 2014]. Most
of the algorithms used in the games industry stem from the A∗
algorithm. The A∗
algorithm, however, faces prolonged search time
in an environment that has many complex obstacles. The real-time
versions of A∗
algorithm inherit this problem and try to solve it using various methods [Cannon et al. 2012; Sturtevant et al. 2010].
Furthermore, A∗
needs discretization of the environment whose
resolution has a strong effect on the search time. Although in [Cannon et al. 2012] the algorithm samples for building a graph, the
number of samples for finding a path between nodes increases such
that it may affect the real-time response time of the algorithm for
taking an action. The response time of the potential field algorithms
is in real-time, however they suffer from trapping into local minima.
Rapidly exploring random trees (RRTs) quickly expand a search
tree in an environment. They are efficient for single-query tasks in
a continuous environment, i.e. when the goal point is fixed. The
different real time variants of RRT path planning either regrow the
whole tree with guidance of the previous iteration for a limited time
113


The initial velocity of the robot and of vehicle 2 is (vx = 30:0 m=s; vy = 0:0 m=s), and
the initial velocity of vehicle 1 is (vx = 23 m=s; vy = 0:0 m=s). The initial velocities are
represented by vectors attached to the center of each vehicle. The grey circles represent
positions of the robot and of the obstacles when they cross each other's path.
First, we computed the time-optimal trajectory using a global search, as discussed
in Section 4.1. The nodes of the tree were generated at 1s intervals. The RAV sets were
17
Exit
robot
vehicle 1
vehicle 2
Figure 17: Trajectory computed by global search.
discretized on average by 12 points, and the tree was expanded to a depth of 5 levels .
Using the Depth First Iterative Deepening algorithm (Korf, 1985) resulted in the
trajectory shown in Figure 17, with the motion time of 3:5 s.
Using the TG heuristic resulted in the trajectory shown in Figure 18. Along this
trajectory, the robot slows down and lets vehicle 2 pass, and then speeds up towards the
exit, behind vehicle 1. The total motion time for this trajectory is 6:07 s.
Using the MV heuristics produced the trajectory shown in Figure 19. Here, the robot
speeds up and passes in front of both vehicles 1 and 2. The total motion time along this
trajectory is 3:56 s. It compares favorably with the time of 3:5 s computed by the global
search.
Using both MV and TG strategies resulted in the trajectory shown in Figure 20.
Here, the robot rst speeds up to pass vehicle 2, then slows down to let vehicle 1 pass,
and then speeds up again towards the goal. The motion time for this trajectory is 5:31 s.
Exit
robot
vehicle 1
vehicle 2
Figure 18: Trajectory computed with the TG strategy.
18
Exit
robot
vehicle 1
vehicle 2
Figure 19: Trajectory computed with MV strategy.
This trajectory was computed using the MV heuristics for 0  t  2:0 s and the TG
heuristics afterwards.
5.3.2. Negotiating Highway Trac
This example considers a "speeding" robot. First, the vehicles are represented by


3
4
5
6
7
8
target 6 5
7 8
1
2
3
4
5
6
7
8
1
2
3
4
5
6
7
8
1
2
3
4
(a) (b)
(c) (d)
Figure 16: Snapshots of the trajectory avoiding static and moving obstacles.
maneuver. Later, due to the change in direction of the obstacle, the front avoidance
maneuver became feasible, which explains the higher speeds at the distal end of the
trajectory shown in Figure 16-b.
5.3. Intelligent Highway Examples
The following examples demonstrate the proposed approach for intelligent vehicles
negotiating highway trac.
5.3.1. Moving to the Exit Ramp
This example consists of a robotic vehicle in the leftmost lane, attempting to reach
the exit ramp on the right, while avoiding other two vehicles that move at constant speeds.


Jet Propulsion Laboratory
California Institute of Technology
Pasadena, CA 91109 USA
y Department of Mechanical, Nuclear and Aerospace Engineering
University of California, Los Angeles
Los Angeles, CA 90024 USA
Abstract
This paper presents a method for robot motion planning in dynamic environments.
It consists of selecting avoidance maneuvers to avoid static and moving obstacles in the
velocity space, based on the current positions and velocities of the robot and obstacles. It
is a rst-order method since it does not integrate velocities to yield positions as functions
of time.
The avoidance maneuvers are generated by selecting robot velocities outside of the
Velocity Obstacles, which represent the set of robot velocities that would result in a
collision with a given obstacle that moves at a given velocity, at some future time. To
ensure that the avoidance maneuver is dynamically feasible, the set of avoidance velocities
is intersected with the set of admissible velocities, dened by the robot's acceleration
constraints. Computing new avoidance maneuvers at regular time intervals accounts for
general obstacle trajectories.
The trajectory from start to goal is computed by searching a tree of feasible avoidance
maneuvers, computed at discrete time intervals. An exhaustive search of the tree yields
near-optimal trajectories that either minimize distance or motion time. A heuristic search
of the tree is applicable to on-line planning. The method is demonstrated for point
and disk robots among static and moving obstacles, and for an automated vehicle in an
Intelligent Vehicle Highway System scenario.
1. Introduction
This paper addresses the problem of motion planning in dynamic environments, such
as robot manipulators avoiding moving obstacles, and intelligent vehicles negotiating freeway trac.
2
Motion planning in dynamic environments is a dicult problem, since it requires
planning in the state-space, i.e. simultaneously solving the path planning and the velocity
planning problems. Path planning is a kinematic problem, involving the computation of
a collision-free path from start to goal, whereas velocity planning is inherently a dynamic
problem, requiring the consideration of robot dynamics and actuator constraints. It was
shown that dynamic motion planning for a point in the plane, with bounded velocity and
arbitrary many obstacles, is intractable (NP-hard) (Canny and Reif, 1987). In addition
to the computational diculty, this problem is not guaranteed to have a solution due to
the uncertain nature of the environment, since a solution computed at time t0 may be
infeasible at a later time (Sanborn and Hendler, 1988).
Motion planning in dynamic environments was originally addressed by adding the


The two images in the upper part of Figure 13 represent the simulation results, while the two images in the lower part represent the experimental results. By comparing them, it can be observed that the speed of the manipulator during the experiment fluctuates slightly but is generally consistent with that in the simulation environment. Overall, although the speed exhibits some fluctuations, it remains around the set speed. Excluding the factors of the manipulator itself, the feasibility of speed control can be verified, and the accuracy of the manipulator to achieve the target posture by speed control can be ensured.


Compared to Figure 7b,c, in Figure 9b, the oscillation of each joint angle of the manipulator in the path section of the shock avoidance region disappears. Likewise, in Figure 9c,d, the velocity and angular velocity are smoother and more continuous, with only transient fluctuations observed at the initial and end positions of the shock avoidance path. This may be due to the approximation of acceleration at the beginning and end positions using the velocity-to-time ratio when using the five-times polynomial planning, leading to slight disturbances in the articulation. Although the obstacle-avoidance path generated by the new position point, which is obtained by increasing the safety radius, is usually inferior to the path generated by oscillation, this approach can result in smoother planning speed and similar operation step length since the number of set interpolation points is calculated based on the distance-to-velocity ratio, and it also avoids frequent tremors and shocks of the manipulator. From Figure 9c, it can be seen that the overall joint angular velocity of the manipulator is smooth and continuous during the operation, and there is no violent and sudden change of velocity when the oscillation occurs, so it can be determined that the method proposed in this paper can effectively eliminate the local oscillation phenomenon. Since the new position points are obtained under the condition of increasing the safety radius, the obstacle-avoidance paths generated by the new position points are not as excellent as the paths generated by the oscillations in most cases. But in this way, the planning speed can be smoothed, while avoiding frequent tremors and shocks of the manipulator.


Tangential velocity function
TVF is defined as

(15)
where
 is unit directional vector of tangential VPF that has been defined in the “VPFs in 3-D space” section. γ is the strength of the potential field obtained by fuzzy logic approach in the same way as
. Figure 10 shows the membership functions of γ.


patches instead of just one node. We tested our algorithm against
CL-RRT which can be considered the state-of-the-art in RRT-based
real-time path planning. Our simulations show that the combination
of retaining the tree with the two methods of rewiring the large tree
in RT-RRT* enables the algorithm to find shorter paths with less
iterations to one or multiple goal points. One should note that, for
the sake of simplicity and memory, we used a grid to speed up our
neighboring node search. Some more sophisticated spatial index
such as a KD-tree would speed up the search even more and allow
using bigger sampling budgets.
RT-RRT* has its limitations, e.g. it requires a large memory capacity because the whole tree is stored at all times. One of the major
limitations of our algorithm is that it only works in a bounded environment. The focused sampling inside an ellipsoid works somewhat in an unrestricted environment but the rewiring suffers if the
distances are large. Thus, the challenges of unbounded and large
distance environments remain to be addressed.
We used C-PBP to move the agent on the planned path and separate the path planning from the motion planning. Although C-PBP
enables the agent to follow the planned path smoothly and plan motions around the obstacles, it regrettably does not provide us with
a model of the obstacles. That is why we made assumptions about
which objects are obstacles and resorted to circumventing the obstacles with a given minimum distance. We deem extracting an
obstacle model a fruitful research direction for the future.
Acknowledgements
This work has been supported by TEKES (The Finnish Funding
Agency For Innovation).
References
BRUCE, J., AND VELOSO, M. 2002. Real-time randomized path
planning for robot navigation. In IROS, IEEE.
CANNON, J., ROSE, K., AND RUML, W. 2012. Real-Time Motion
Planning with Dynamic Obstacles. In Symposium on Combinatorial Search.
GAMMELL, D, J., SRINIVASA, S, S., BARFOOT, AND D, T. 2014.
Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic. In IROS, IEEE.
GUTMANN, J.-S., FUKUCHI, M., AND FUJITA, M. 2005. Realtime path planning for humanoid robot navigation. In IJCAI.
HAM¨ AL¨ AINEN ¨ , P., RAJAMAKI ¨ , J., AND LIU, C. K. 2015. Online
Control of Simulated Humanoids Using Particle Belief Propagation. In Proc. SIGGRAPH ’15, ACM.
KARAMAN, S., AND FRAZZOLI, E. 2011. Sampling-based Algorithms for Optimal Motion Planning. Int. J. Rob. Res..
KAVRAKI, L. E., Sˇ VESTKA, P., LATOMBE, J.-C., AND OVERMARS, M. H. 1996. Probabilistic roadmaps for path planning
in high-dimensional configuration spaces. IEEE Trans. Robot.
Autom..
KHATIB, O. 1986. Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics
Research.
LAVALLE, S. M. 1998. Rapidly-Exploring Random Trees: A new
Tool for Path Planning.


Figure 1. Architecture of robots collaboration with collision avoidance.
The geometry and parameters of robots are built offline. Joint angles of robots are retrieved from their encoders. The location and geometry of obstacles are known here by a binocular vision system that was introduced in the study by Huang.33 All these information are shared in local area network (LAN) in real time.34 Based on a simplified model of robots and obstacles, the nearest distance is estimated among robots and obstacles. Then a set of joint velocities is generated by collision-free algorithms and the robot motion control commands are sent to the controller to operate the manipulators, which are updated at a specified time interval until the manipulator reaches the goal.
The article is organized as follows: The “Simplified model and distance estimating” section describes the simplified models of objects by swept sphere volumes (SSVs) and the estimation of the minimum distance. An improved collision avoidance algorithm based on VPF is presented in the “An improved collision-free algorithm based on VPF” section. The “Simulations and experimentation” section expounds the implementation of simulation and experiments and the “Conclusions” section devotes to the conclusions.
Simplified model and distance estimating
The effectiveness of the proposed algorithm largely depends on the method of estimate of distance between objects. Using SSV bounding box technology that can be applied to construct a hierarchical tree and approach the object geometry gradually to represent complex objects, the minimum distance detection can be performed more efficiently and accurately.
Swept sphere volumes
If Q denotes swept path, S denotes primitive, then the collection of points can be formed by S swept Q as follows
(1)
where Sc denotes S at location c. Q has three geometric primitives which are points, line segments, and rectangles.
The swept spheres formed by primitives are point swept spheres (PSSs), line segments swept spheres (LSSs), and rectangle swept spheres (RSSs). As a result, the geometric model of the manipulator can be simplified by a list of SSVs.
Hierarchies of simplified model
According to the characteristics of manipulators, a method of two-layer hierarchies of simplified model is proposed as follows:
Step 1: Layer one
As shown in Figure 2(a), the manipulator is only represented by a RSS.


Figure 12. Trajectories with different RVFs. (a) Robot with a speed of 10 mm/s. (b) Robot with a speed of 50 mm/s. RVFs: repulsive velocity functions.
Repulsive velocities in the literature23,28–30 are related to
, and RVF1, RVF2, and RVF3 correspond to r = 0, r = 1, and r = 2, respectively. RVF4 from the study by Wu32 changes with
.
As shown in Figure 12, the trajectories of robot are oscillating near the obstacle at different levels when applied in the RVF1, RVF2, RVF3, and RVF4, although the oscillation can be improved as the robot speed increases for the RVF3. For the proposed function in this article, the trajectories of the robot keep the shortest and smoothest path while avoiding the obstacle whether the robot speed is low or high.
TVF with different directions
In this simulation, an obstacle of cylinder with 50 mm radius and 100 mm height is located at O (200, 200, 0). Plane L is perpendicular to
 in 3-D space. As shown in Figure 13,

,

,

,

, and

 in the plane L are examples of tangential velocity which is perpendicular to
 as usual, and
 is the defined tangential velocity in this article.


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g008a.jpg
An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g008b.jpg
Figure 8
Joint velocity under local oscillation. (a) The speed variation of joint 1–3; (b) The speed variation of joint 4–6.


𝜃˙=piv(𝐽−1𝑖)⋅𝑉𝑟𝑒𝑝(𝑥,𝑦,𝑧)+piv(𝐽−16)⋅𝑉𝑎𝑡(𝑥,𝑦,𝑧)
(4)
2.3. Local Oscillation and Virtual Target Point
Since the artificial potential field method is based on the idea of the potential field method, whether the force potential field method or the velocity potential field method is utilized, the direction of the force control or velocity control of the manipulator is unidirectional and abrupt. Therefore, the speed of the manipulator will oscillate with the appearance and disappearance of the repulsive speed. When the manipulator experiences repulsion from an obstacle in a direction opposite to the attraction of a target object, or when the angle between the two directions is excessively large, it can result in the manipulator repeatedly leaving the repulsive velocity potential field range during obstacle avoidance and re-entering due to the attraction, causing local oscillation, as depicted in Figure 4. The virtual target-point-based processing technique is one of the key strategies to cope with the local oscillation problem created by the artificial potential field method. Other processing strategies have certain inherent issues. For example, the computation is more complicated, the real-time performance is not acceptable, and it generates a huge instantaneous force or velocity on the robotic arm, which compromises the safety of the operation of the robotic arm. Therefore, in this study, the virtual target-point-based processing technique, which is widely utilized in this research area, is employed to solve the local oscillation problem. This paper proposes a strategy for mitigating the oscillation in single-point obstacle path planning by using virtual target points.


a segment of the boundary of the velocity obstacle V OB, @(V OB). Subset Sd, instead,
does not include in its boundary any segment from the boundary of V OB. From Lemma 1,
velocities with tip on these segments generate trajectories grazing B, and therefore only
subsets Sf and Sr include tangent velocities.
Tangent velocities separate collision velocities from avoidance velocities, and therefore, a velocity vA 2 fSf ; Srg, whose tip is away from the boundary @(V OB) will generate
a trajectory passing at a certain distance from Bj , on the same side of the tangent trajectory.
Similarly, trajectories generated by velocities vA 2 Sd cannot be tangent to B in
a nite time, since the boundary of Sd is the velocity parallel to vB. Therefore, if this
subset exists in RAV, it contains all the velocities diverging from B.
Proof of Theorem 1:
From Lemma 2, each velocity obstacle V OBj divides the set of reachable avoidance velocities RAV into three non-overlapping subsets. Then, m obstacles, Bj (j = 1; : : : ; m),
25
generate 3m subsets, that are partially overlapping. Let us assume that RAV is divided
into l non-overlapping subsets RAVi i = 1; : : : ; l.
Each subset, @(RAVi), may include segments from the boundaries of some velocity obstacles V OBj
, @(V OBj ). From Lemma 2, all velocities in the same subset RAVi
will generate the same type of avoidance maneuver. Then, if the boundary of a RAVi
@(RAVi), includes segments from the boundary of n dierent velocity obstacles V OBj (j =
1; : : : ; n; n  m), all velocities vA 2 RAVi will generate trajectories avoiding each of the
n obstacles Bj (j = 1; : : : ; n; n  m) with the maneuver determined by the portion of
@(V OBj ) included in @(RAVi). Then, each subset RAVi corresponds to a specic maneuver type, represented by indices i
j
, where the index i represents the type of avoidance
velocity, i.e. (i=f,r,d), and j ; (j = 1; : : : ; m) is the index of the obstacle.
Some obstacles do not contribute directly to the boundary of a RAVi because a
successive V OBj has covered the corresponding segment of the boudary of the RAVi.
In this case the correct sequence of avoidances is generated incrementally during the
computation of the RAVi.
26
(a) (b)
A
^
l
aa
RV
(c) (d) VOB
A
^
A
^


Numerous researchers have improved the VPF; however, current methods do not deal with the dynamic obstacle-avoidance problem of manipulators in human-robot collaboration well. These methods cannot match the real-time and safety criteria for manipulator obstacle avoidance in robot collaboration scenarios. Hence, in this research, an improved VPF algorithm is proposed for the dynamic obstacle-avoidance problem of manipulators in human-robot collaboration settings. This method eliminates the local oscillation problem in the standard VPF method and makes the path smoother. It addresses the safety and real-time criteria of dynamic obstacle avoidance in human-robot collaboration situations, thereby increasing the efficiency of human-robot cooperation. The rest of this paper is structured as follows. In Section 2, a path-planning algorithm for a manipulator in 3D space based on the velocity potential field is proposed. The performance of the proposed algorithm and the results will be analyzed in Section 3. The whole discussion is summarized, and a conclusion is presented in Section 4.


Figure 13. TVF with different directions.
The robot moves from point S (0, 400, 0) to point G (400, 0, 100). The simulation results are shown in Figure 14 that the path of the robot keeps the shortest by applying
. Compared to all other tangential velocity in the plane L,

 lies the smallest angle with

, so the robot with

 has the second shortest path after
.


As the manipulator approaches the target point, the attraction speed gradually decreases to zero. This S-curve-like velocity design scheme adjusts its velocity amplitude to approach the target in almost a straight line when there are no obstacles, avoiding the problem of oversized initial velocity or limited velocity, which fails to solve the problem of sudden change in initial speed while ensuring the mission’s efficiency. Figure 2a shows the Cartesian velocity profile model in the x-direction. During the operation of a manipulator, there is a situation in which a repulsive speed potential field is simultaneously applied to the manipulator by multiple obstacles, as shown in Figure 3. When multiple obstacle points generate repulsion speed potentials for the manipulator, the priority obstacle avoidance lies in selecting the speed of the joint closest to the obstacle, as outlined in Figure 3. Abrupt entry or exit from the repulsive velocity potential field will cause a sudden change in the total velocity imparted on the manipulator, which may cause an impact. The repulsive velocity potential field is partitioned around each obstacle. The repulsion zone is within the potential field with 𝑑𝑠 representing its radius, while the buffer zone outside the potential field is established with 𝑑𝑓 as the radius. The buffer velocity function alters the repulsion velocity close to zero when the control point enters the buffer zone but remains outside the repulsion zone, minimizing the effects on the manipulator caused by sudden velocity changes. The entrance into the repulsion zone is subjected to normal repulsive velocity.


Figure 2. The simplified model of robot. (a) Layer one. (b) Layer two.
Step 2: Layer two
Layer two will be checked when the potential collision is not eliminated in layer one. In this layer, the manipulator is represented by mixed SSVs, as shown in Figure 2(b).
Distance checking
As shown in Figure 3, there are two SSVs (two LSSs in Figure 3(a), one LSS and one RSS in Figure 3(b) in the space. Ci and Cj are SSVs, Li and Lj are geometric primitives, ri and rj are the radius of Li and Lj, respectively. The approximate distance between the two SSVs is
(2)
where
 is the distance between the primitives Li and Lj, which can be obtained through space analytic geometry.35


Open access
Research article
First published online July 13, 2018
A novel non-collision trajectory planning algorithm based on velocity potential field for robotic manipulator
X Xu, Y Hu, […], and PS Guo+2View all authors and affiliations
All Articles
https://doi.org/10.1177/1729881418787075


Information for
Authors
Editors
Librarians
Promoters / Advertisers
Researchers
Reviewers
Societies
Frequently asked questions


The agent was modeled as a disk whose diameter equals
the length of the grid cell. The confguration is defned as
(x, y, θ, vel) where x, y are the coordinates of the cell, θ ∈
{0
◦
, 90◦
, 180◦
, 270◦} is the orientation, and vel ∈ {0, 2} is
the velocity of the agent.
The following motion primitives were defned: accelerating, decelerating, and uniform. Accelerating (decelerating)
primitive: move with the fxed acceleration of 0.5 cell/s
2
(−0.5 cell/s
2
) from a confguration with zero (2 cell/s) velocity until reaching maximum (zero) velocity. The agent
traverses four cells this way without changing its orientation. Uniform motion primitive: go with the maximum velocity (2 cell/s) one cell forward (the orientation does not
change). Additionally, if the velocity is zero, the agent can
12335
25 20 15 10 5 4 3
#Free_Cells/#MO
0
20
40
60
80
100
Success Rate(%)
room-64-64-16
25 20 15 10 5 4 3
#Free_Cells/#MO
empty_64_64
25 20 15 10 5 4 3
#Free_Cells/#MO
warehouse-10-20-10-2-2
25 20 15 10 5 4 3
#Free_Cells/#MO
random128_10_0
25 20 15 10 5 4 3
#Free_Cells/#MO
Sydney_2_256


reachable
velocities
kinematic constraints
reachable velocities due to
minimum
turning
radius
Figure 8: Dynamic versus kinematic constraints.
accelerations prevent the vehicle from moving sideways at non-zero forward speeds, thus
automatically satisfying the non-holonomic constraints (Shiller and Sundar, 1996). For
this reason, we will disregard the non-holonomic kinematic constraints in computing the
feasible avoidance maneuvers.
3.2. Structure of the Avoidance Maneuvers
Avoidance maneuvers can be classied according to their position with respect to the
moving obstacle. In general, the avoidance velocities form two disjoint sets separated by
the set of colliding velocities, as was shown in Figure 7. The boundary between colliding
and non-colliding velocities consists of the velocities laying on the two lines f and r ,
generating a maneuver tangent to Bb
at some point P 2 @B. To choose the structure of
an avoidance maneuver, it is then necessary to determine on which side of the obstacle
each maneuver will pass.
We identify the front and rear sides of Bb
by attaching a coordinate frame (X,Y) to
the center of Bb
, oriented with its X axis coinciding with the velocity vB. The Y axis then
partitions the boundary of Bb
, @B, at points Yf and Yr into the front semi-circle, @Bf ,
which intersects the positive X axis, and the rear semi-circle, @Br , as shown in Figure 9.
The following Lemma states that tangent maneuvers can be generated only by relative
velocities laying on f and r , and that the actual tangency points of these maneuvers
are dierent from the tangency points Tf and Tr of f and r , since they depend on the
absolute velocities of A and B. Assuming no rotations, Figure 10 shows schematically the
position of the tangency point P at time t0, when the avoidance maneuver is computed,
and at time t1, when Ab
reaches Bb
.
Lemma 1: Robot A is tangent to obstacle B at some point P 2 @B i it follows a
trajectory generated by vA corresponding to vA;B 2 ff ; rg. The tangency sets in @B
10
Tr


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g001.jpg
Figure 1
Obstacle-avoidance velocity generation of Mico2 manipulator.


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g005.jpg
Figure 5
Diagram of the virtual target-point setting method.


Figure 7. v’s membership function.
 is divided into five fuzzy sets: Very Small (VSL), Small (SM), Medium (ME), Big (BG), and Very Big (VB). According to simulation results and experimental experiences, the membership functions of kp and kd are shown in Figure 8 and Figure 9, respectively.


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g003.jpg
Figure 3
Multi-obstacle repulsive velocity potential field model.


3.1.2. Simulation of Local Oscillation
In the absence of oscillation strategy processing, multiple obstacles are placed in the manipulator workspace until the manipulator experiences localized oscillations during motion. This can verify the ability of the robot to autonomously avoid obstacles and observe oscillations in its path when passing near obstacles. The rejection velocity function parameters a, b, and c are specified, and the step duration is set to t = 0.1 s. The simulation results are shown in Figure 7a.


Permission to make digital or hard copies of all or part of this work for personal or
classroom use is granted without fee provided that copies are not made or distributed for
profit or commercial advantage and that copies bear this notice and the full citation on the
first page. Copyrights for components of this work owned by others than ACM must be
honored. Abstracting with credit is permitted. To copy otherwise, or republish, to post on
servers or to redistribute to lists, requires prior specific permission and/or a fee.
Request permissions from Permissions@acm.org.
MIG '15, November 16 – 18, 2015, Paris, France.
© 2015 ACM. ISBN 978-1-4503-3991-9/15/11…$15.00
DOI: http://dx.doi.org/10.1145/2822013.2822036
RT-RRT*: A Real-Time Path Planning Algorithm Based On RRT*
Kourosh Naderi∗
Joose Rajamaki ¨
† Perttu Ham¨ al¨ ainen ¨
‡
Aalto University
Figure 1: Bold lines and shaded circles denote paths from the agent to different goals and dynamic obstacles, respectively. In Figure (a) the
tree is growing and a path to the goal point is found. In Figures (b) and (c) the goal point is changed on the fly. In both cases since the tree
root moves with the agent and the tree covers most of the environment, the paths to the changed goals are returned quickly. In Figure (d) the
agent has reached the goal point and rewiring of the nodes based on the current location of the tree root has generated the minimum length
paths to others nodes of the tree. The crossed points denote the Rewiring Circle.
Abstract
This paper presents a novel algorithm for real-time path-planning in
a dynamic environment such as a computer game. We utilize a realtime sampling approach based on the Rapidly Exploring Random
Tree (RRT) algorithm that has enjoyed wide success in robotics.
More specifically, our algorithm is based on the RRT* and informed RRT* variants. We contribute by introducing an online tree
rewiring strategy that allows the tree root to move with the agent
without discarding previously sampled paths. Our method also does
not have to wait for the tree to be fully built, as tree expansion and
taking actions are interleaved. To our knowledge, this is the first
real-time variant of RRT*.
We demonstrate our method, dubbed Real-Time RRT* (RT-RRT*),
in navigating a maze with moving enemies that the controlled agent
is required to avoid within a predefined radius. Our method finds
paths to new targets considerably faster when compared to CL-RRT,
a previously proposed real-time RRT variant.
CR Categories: I.2.8 [Computing methodologies]: Artificial
Intelligence—Problem Solving, Control Methods, and Search I.2.9
[Computing methodologies]: Artificial Intelligence—Robotics
Keywords: Real-time path planning, RRT*, dynamic environment, multiple-query planning


Figure 20. Trajectories of robots and moving obstacle. (a) Trajectories at the projection of XY plane. (b) Magnification of trajectories near obstacles.
Conclusions
In this article, a real-time algorithm based on VPF is introduced to coordinate collision-free trajectories for robotic manipulators in 3-D space. The robot velocity is modeled by AVF, RVF, and TVF, which plan an optimal collision-free trajectory. Compared with the applications of the current methods based on VPF, the AVF with a saturation function makes the robot reach the goal faster without oscillation, the RVF with a fuzzy spring damping system has enhanced the optimum trajectories as well as the adaptability to different velocities while avoiding the obstacle. A new method of TVF is created to get the shortest path in 3-D space, considering the moving direction of the obstacle and the relative positions of obstacles, target, and robots currently. The control algorithm of multi-robots cooperation takes robot priority strategies into avoiding collision among robots that makes it more flexible in path planning and gets more coordinating paths based on the task importance and the time spent of every robot working.
The desired outcomes of the algorithms are verified in the simulations, which not only solve the problems of local minima around obstacle and oscillation near the obstacle and goal but also show better performances in trajectory or path planning. An experiment of the two robots working around a stationary obstacle and a moving obstacle is executed, and the results illustrate the effectiveness of the proposed method in real-time collision avoidance among robots and stationary or moving obstacles. Future works on this algorithm include optimizing the collision path further in consideration of moving obstacle velocity magnitude, and investigating its feasibility in narrow channels.
Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.
Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work has been supported by the Science and Technology Planning Projects of Guangdong Province (2014B090920001).
ORCID iD
JM Zhai https://orcid.org/0000-0001-9476-5618
References
1. Kinugawa J, Kawaai Y, Tanaka Y, et al. Collision risk reduction system for assembly task partner robot. In: 2012 IEEE/ASME international conference on advanced intelligent mechatronics, Kaohsiung, Taiwan, 11–14 July 2012, pp. 280–285. IEEE.
GO TO REFERENCE
Google Scholar
2. Wangmanaopituk S, Voos H, Kongprawechnone W. Collaborative nonlinear model-predictive collision avoidance and path following of mobile robots. In: ICROS-SICE international joint conference 2009, Fukuoka, Japan, 18–21 August 2009, pp. 3205–3210.
GO TO REFERENCE
Google Scholar
3. Zhang WJ, Yuan J, Li Q, et al. An automatic collision avoidance approach for remote control of redundant manipulator. In: 2008 IEEE international conference on information and automation, Zhangjiajie, Hunan, China, 20–23 June 2008, pp. 809–814. IEEE.
GO TO REFERENCE
Google Scholar
4. Duchon F, Babinec A, Kajan M, et al. Path planning with modified a star algorithm for a mobile robot. Model Mech Mechatron Syst 2014; 96: 59–69.


Figure 16. Collision avoidance processes (down: front view, up: top view). (a) t = 0 s. (b) t = 2 s. (c) t = 2.5 s. (d) t = 3 s. (e) t = 3.8 s. (f) t = 5 s. (g) t = 6 s. (h) t = 9 s.


Sr
Sd
Sf
VOB
A
^
1
2 3
4
5
6
1
2 3
4
5
6
7
8
9
1
2 3
4
5
6
7
8
9
λ
f
r
λ
λ
f
r
λ
Sd
Sd
Sd
Snd
Figure 24: The computation of the Reachable Avoidance Velocities RAV.


contains the path from x0 to xgoal, we made it possible to rewire the
path to xgoal very quickly. Note that, when rewiring is done on the
path from x0 to xgoal, we need to update the path again (see Section 4.2). Using Qs in Algorithm 5, allows us to grow the Rewiring
Circle between iterations. As the Rewiring Circle grows (supplemental video 00:40) every node inside this circle is rewired. Thus,
on the already expanded tree, a minimum path to every node in the
circle is created due to the circle that grows from x0. Note that,
Algorithm 5 only rewires on the already expanded tree and since
the tree is not complete in the beginning, it is possible that when a
new node is added to the tree, Algorithm 4 creates paths with lower
cis to the nodes in the circle. Qs gets restarted when x0 is changed
or a dynamic obstacle blocks a node with lower ci than cis of the
nodes on the Rewiring Circle. In Algorithm 5, restarting Qs means
we need to rewire nodes from x0 again (line 10).
4.2 Path Planning
Line 7 in Algorithm 1 uses Algorithm 6 to plan a k-step path from
x0. Algorithm 6 plans the path in two ways: 1) when tree reaches
xgoal (lines 2-4) which happens when we expand the tree using Algorithm 3; 2) when the tree has not reached xgoal (lines 6-10). In
the first way, the path from xgoal up to x0 is in the tree and thus we
only need to update the path (line 3) when rewiring the path is done
using rewiring algorithms in Section 4.1.2. In the second way, we
plan a path to get as close as possible to xgoal on the tree. We use
cost function fi = ci + hi to get close to xgoal as well as to take
a short path on the growing tree. Thus, using fi can trap us in local minima. To prevent trapping in local minima and enable path
planning to visit other branches, we plan a k-step path using fi at
each iteration (lines 6,7) and block the already seen nodes (line 10)
by setting their his to infinity. When path planning reaches a node
that could not go any further (line 8), we return the planned path
and block that node (lines 9,10). Then, we update the best already
found path if the planned path leads us to a location closer to xgoal
(line 11). However, the agent follows the best path if it leads to
some place closer than the current place of the agent (line 12).
Algorithm 6 Plan a Path for k Steps
1: Input: T , xgoal
2: if Tree has reached xgoal then
3: Update path from xgoal to x0 if the path is rewired
4: (x0, ..., xk) ← (x0, ..., xgoal)
5: else
6: for xi ∈ (x1, x2, ..., xk) do
7: xi=child of xi-1 with minimum fc=cost(xc)+H(xc)
8: if xi is leaf or its children are blocked then


Figure 3. Distance estimate of SSVs. SSVs: swept sphere volumes. (a) LSS to LSS. (b) LSS to RSS.
Assuming object #p is represented by mp SSVs and they are

, object #q is represented by mq SSVs and they are

. Then the matrix of distance checking is

(3)
where
 is the distance of SSV

 and

,
.
The minimum distance
 between object #p and object #q is
(4)
An improved collision-free algorithm based on VPF
VPFs in 3-D space
VPFs are defined in 3-D workspace of manipulators as shown in Figure 4. Attractive velocity function (AVF)

 ensures end-effector
 head toward goal point
, whose speed is associated with the distance between N and G and the direction is from N to G.


The velocity in the oscillation section displays apparent fluctuations in Figure 8, with joint velocities oscillating in the oscillation area, where the amplitude of oscillation increases and then decreases, corresponding to the manipulator traveling through the buffer zone, the repulsion zone, back through the buffer zone, and finally leaving the repulsion potential field. Therefore, the repulsive velocity potential field buffer is verified to be effective in the simulation, as the velocity amplitude does not change abruptly at the beginning or the end of the oscillation. However, because of the absence of a handling strategy for the oscillation situation described in the preceding section, the simulation was unable to avoid oscillation.


The obstacle is a sphere with a radius of 0.04 m with spatial location coordinates 𝑜𝑏𝑠=[0.4−0.30.3]. To further examine the variations in joint velocity and arm end velocity in the absence of obstacles, the step planning time for the manipulator is modified to t = 0.01 s. Fine interpolation is conducted to obtain more precise results, and the velocity variation is shown in Figure 6b,c.


Crossref
ISI
Google Scholar
30. Hu M, Li CH. Application in real-time planning of UAV based on velocity vector field. In: International conference on electrical and control engineering, Wuhan, China, 26–28 June 2010, pp. 5022–5026. IEEE.


o
i+1,k,2
Figure 13: Tree representation for the global search.
generating the avoidance maneuvers, are the velocities computed by discretizing RAV.
The search tree is formally dened as follows:
nj (ti) = fxj ; RAV (ti))g (9)
oj;l(ti) = fvl(ti) j vl(ti) 2 RAVj (ti)g (10)
ej;k(ti) = f(nj (ti); nk(ti+1)) j nk(ti+1) = nj (ti) + (oj;l T )g (11)
where nj if the jth node at time ti , RAVj (ti), is the reachable velocity set computed for
node nj , oj;l is the lth operator on node j at time ti, and ej;k is the branch between node
nj at time ti , and node nk at time ti+1.
To keep the computation manageable, each avoidance set RAV(ti) is discretized by a
grid. The positions reached by the robot at the end of each maneuver are the successors of
node nj (ti). A node nj (ti) is completely expanded when all the operators oj;l 2 RAVj have
been applied. The resulting tree has a constant time interval between nodes, and a variable
branch number that depends on the shape of each RAV. By assigning an appropriate
cost to each branch, this tree can be searched for the trajectory that maximizes some
objective function, such as distance traveled, motion time, or energy, using standard
search techniques (Nilsson, 1980; Pearl, 1985). Figure 13 shows schematically a subtree
of a few avoidance maneuvers.
Since the avoidance maneuvers are generated using the velocities in RAV, each segment of the trajectory avoids all the obstacles that are reachable within the specied time
horizon. This excludes trajectories that might, on some segment, be on a collision course
with some of those obstacles, and that would avoid them on a later segment. Therefore,
this method generates only conservative trajectories. Trajectories excluded from the computation can be generated by considering a shorter time horizon, or by computing the
time-minimal trajectories (Fiorini, 1995).
14
A
(a)
Goal
A
VO
Max angle (b)
Goal
VO
v
A
RAV
v
(c)
Goal


3
4
6 5
7 8
target
1
2
3
4
6 5
7 8
start start
(a) (b)
Figure 15: Trajectories avoiding static obstacles.
5. Examples
This Section presents several examples of navigation in static and dynamic environments using the Velocity Obstacle approach.
5.1. Avoidance of static obstacles
The rst two examples demonstrate the applicability of this approach to static environments. Here, a point robot avoids eight circular obstacles, arranged as shown in
Figure 15, starting from rest from two dierent points. The trajectories shown in Figure 15 were computed every 1 s using the MV heuristics, and time horizon Th = 10 s. The
trajectory in Figure 15-a rst follows the boundary of the velocity obstacle of obstacle 1,
then switches to obstacle 5. Similarly, the trajectory in Figure 15-b switches from obstacle 3, to 2, and nally to 5. In this case, obstacle 1 was not obstructing the target after
avoiding obstacle 2. In both cases, setting the time horizon higher, to Th = 20 s, resulted
in no solution since the reachable avoidance velocities RAV at the the initial positions
were completely covered by the V O of the static obstacles.
5.2. Avoidance of xed and moving obstacles
This example demonstrates avoidance of static and moving obstacles that move along
circular trajectories. The robot starts from rest at the center, as shown in Figure 16-a, rst
avoiding the small static obstacles, and then avoiding the large moving obstacles, using
the MV heuristics. The trajectory is shown in three snapshots in Figures 16-(b,c,d),
corresponding to times t = 24 s, t = 29 s, and the nal time t = 37:6 s, respectively. After
avoiding obstacle 1, the robot moves to avoid the incoming obstacle 8. It aims at being
tangent to obstacle 8, which explains the initial distance from the obstacle in Figures 16-
(b). The robot is eventually tangent to obstacle 8, as shown in Figure 16-c. From that
point on, the robot continues towards the target, as shown in Figure 16-d.
Here the robot slows down while approaching obstacle 8, as represented by the short
distance between the circles in Figure 16-b. This occurred due to the linear approximation
of the obstacle's trajectory, which at those points made rear avoidance the only feasible
16
start 1
2


edge, w(e) ∈ N.
Based on the source/target velocity, each motion (edge)
can be classifed as either accelerating, decelerating, or uniform. In the latter case, the agent’s velocity at the target
confguration is the same as in the source one, while in the
12331
frst two cases, it changes (increases and decreases, respectively). The presence of accelerating/decelerating motions
makes our problem distinguishable from the works that assumed instantaneous acceleration1
.
Besides the agent and the static obstacles, the environment is populated by a fxed number of moving obstacles.
We assume that their trajectories are known and converted
(by an auxiliary procedure) to the collision and safe intervals
associated with the graph vertices and edges; i.e., for each
vertex, a fnite sequence of non-overlapping time intervals
SI(v) is given, which is the sequence of the safe intervals
at which the agent can be confgured at v without colliding with any moving obstacle. Similarly, safe intervals are
defned for each edge, SI(e). If the agent executes a move
e at any timestep outside SI(e), a collision with a moving
obstacle occurs.
A (timed) path for the agent, or a trajectory, is a sequence
π = (e1, t1),(e2, t2), ...(eL, tL), where ti ∈ T is the time
moment the motion defned by the edge ei
is started. The
cost of the trajectory is the timestep when the agent reaches
the fnal vertex, cost(π) = eL + w(eL). A trajectory is feasible if the target vertex of ei matches the source vertex of
ei+1 and ti+1 ≥ ti + w(ei) (with t0 = 0). Moreover if
ti+1 > ti + w(ei), then the velocity at the source of ei+1
must be zero.
A feasible trajectory can also be seen as a plan composed
of the move and wait actions, where the move actions are
defned by the graph edges and the wait actions might occur
at the vertices where the agent’s velocity is zero. The duration of the wait action occurring after the i-th move action is
computed as δ = ti+1 − (ti + w(ei)). As w(ei) is integer, δ
is integer as well.
A collision between the trajectory π and the dynamic obstacles occurs iff either of the conditions occur: i) the agent
starts executing some motion primitive e at the timestep
which is outside of SI(e); ii) according to π, the agent waits
at a vertex v outside of SI(v).
The problem now is to fnd a feasible collision-free trajectory π that transfers the agent from start to goal on a given
G (with the annotated safe intervals of vertices/edges). In
this work, we are interested in solving this problem optimally, i.e., in reaching the goal as early as possible.


PrivacyPrivacy
Skip to main content
Sage Journals Home
Search this journal
Enter search terms...
Enter search terms...
Advanced search
Access/Profile
Cart


The spatial velocity transformation for a manipulator typically requires a Jacobi non-square form, which consequently needs a solution via a non-square format Jacobi matrix [26]. Currently, the most popular method used in practical applications to obtain the pseudo-inverse solution is the Singular Value Decomposition (SVD) algorithm, which is used in this paper. This paper adopts an attractive velocity potential field as the global potential field, which defines the maximum attractive velocity boundary around the target object. Additionally, a local repulsive velocity potential field is established, centered around the obstacles. The manipulator in space is influenced by the attractive velocity 𝑉𝑎𝑡𝑡 and moves towards the target object. If the manipulator enters the range of the local potential field, it is influenced by the repulsive velocity 𝑉𝑟𝑒𝑝 and moves away from the obstacles. Finally, the overall path planning is completed. The overall potential field model is depicted in Figure 1.


International Journal of Advanced Robotic Systems
ISSN: 1729-8806
Online ISSN: 1729-8814
About SageContact usCCPA - Do not sell my personal informationCCPA
Privacy Policy
Copyright © 2024 by SAGE Publications Ltd, unless otherwise noted. Manuscript content on this site is licensed under Creative Commons Licenses.


About
About Sage Journals
Accessibility guide
Historical content
Advertising disclaimer
Permissions
Terms of use
Sage discipline hubs
Sage microsites


v = 1
t0 = 12
0 1 2 3 4 5 6 7 8 9
Distance (m)
0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
..
Time(s)
Figure 1: An example of the challenging path planning instance with dynamic obstacles.
In our work, we adopt an assumption (more realistic from
the practical perspective) that decelerating to a full stop and,
similarly, accelerating from the full stop takes time. In other
words, the agent is subject to kinodynamic constraints. This
makes SIPP inapplicable as shown in Fig. 1. In this example, the environment is discretized to the 3 × 10 grid,
where the distance between the centers of the grid cells is
1 m, and the agent has to go from the leftmost cell B0 to
the rightmost one B9. Moreover, two dynamic obstacles are
present in the environment that block cells B5 and B7 during the intervals shown in black in the bottom part of the
fgure. Assume that the agent’s cruising speed is 1 m s−1
. At
this speed, it takes 1 s to move between the cells. Furthermore, the maximum acceleration/deceleration of the agent
is 0.5 m s−2
, which means that accelerating to the cruising
speed from the full stop takes 2 s and 1 m. Therefore, when
starting to move from the full stop, the agent will arrive at
the neighboring cell not in 1 s, but rather in 2 s. If the algorithm ignores that, like the original SIPP, which assumes
The Thirty-Seventh AAAI Conference on Artificial Intelligence (AAAI-23)


Google Scholar
5. Zidane IM, Ibrahim K. Wavefront and a-star algorithms for mobile robot path planning. In: 2017 3rd international conference on advanced intelligent systems and informatics, Cairo, Egypt, 9–11 September 2017, pp. 69–80. IEEE.
Google Scholar
6. Zhai JM, Liu K, He H, et al. An efficient approach for collision-free trajectory planning using adaptive velocity vector field algorithm. In: 2017 international conference of mechanical design and the 19th annual conference on mechanical design, Beijing, China, 19–21 December 2017, pp. 1169–1190. IEEE.
Google Scholar
7. Wang DJ. Indoor mobile-robot path planning based on an improved A* algorithm. Press Tsinghua Univ 2012; 52: 1085–1089.
GO TO REFERENCE
Google Scholar
8. Verwer BJH. A multiresolution work space, multiresolution configuration space approach to solve the path planning problem. In: Proceedings of the 1990 IEEE international conference on robotics and automation, Cincinnati, OH, USA, 13–18 May 1990, pp. 2107–2112. IEEE.
GO TO REFERENCE
Google Scholar
9. Ademovic A, Lacevic B. Path planning for robotic manipulators via bubbles of free configuration space: evolutionary approach. In: 2014 22nd Mediterranean conference on control and automation, Palermo, Italy, 16–19 June 2014, pp. 1323–1328. IEEE.
GO TO REFERENCE
Google Scholar
10. Ademovic A, Lacevic B. Path planning for robotic manipulators using expanded bubbles of free C-space. In: 2016 IEEE international conference on robotics and automation, Stockholm, Sweden, 16–21 May 2016, pp. 77–82. IEEE.
GO TO REFERENCE
Google Scholar
11. Wang N, Chen P. Path planning algorithm of level set based on grid modeling. In: 2010 international conference on computer design and applications, Qinhuangdao, Hebei, China, 25–27 June 2010, pp. V5508–V5510. IEEE.
GO TO REFERENCE
Google Scholar
12. Zhang B. An algorithm for mobile robot path planning based on a 3D grid map. J Xian Jiaotong Univ 2013; 47: 57–61.
GO TO REFERENCE
Google Scholar
13. Wang JJ, Kai C. Path planning for robot based on grid algorithm. Agri Equip Vehicle Eng 2009; 4: 14–17.
GO TO REFERENCE
Google Scholar
14. Qureshi AH, Ayaz Y. Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments. Robot Auto Syst 2015; 68: 1–11.
GO TO REFERENCE
Crossref
Google Scholar
15. Jorge N, Slawinski E, Mut V, et al. Online path planning based on rapidly-exploring random trees. In: IEEE-ICIT 2010 international conference on industrial technology, Vina del Mar, Chile, 14–17 March 2010, pp. 1451–1456. IEEE.
GO TO REFERENCE
Google Scholar
16. Sertac K. Anytime Motion Planning using the RRT. In: 2011 IEEE international conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp. 1478–1483. IEEE.
GO TO REFERENCE
Google Scholar
17. Khatib O. Real-time obstacle avoidance for manipulators and mobile robots. Int J Robot Res 1986; 5(1): 90–98.
GO TO REFERENCE
Crossref
ISI


5: xs=PopFirst(Qs), Xnear=FindNodesNear(xs, XSI)
6: for xnear ∈ Xnear do
7: cold=cost(xnear), cnew=cost(xs)+dist(xs, xnear)
8: if cnew < cold and line(xs, xnear) ∈ Xfree then
9: ET ← (ET \{Parent(xnear), xnear})∪{xs, xnear}
10: if xnear is not pushed to Qs after restarting Qs then
11: Push xnear to the end of Qs
12: until Time is up or Qs is empty.
4.1.2 Rewiring The Tree
Rewiring is done when a node (xi) gets a lower cost-to-reach ci
value by passing from another node instead of its parent. Thus, in
our algorithm rewiring should be done around the new node (xnew)
that is added (same as RRT*) as well as around the already added
nodes (as opposed to RRT*) due to changing the tree root (x0) and
116
dynamic obstacles. When x0 or any dynamic obstacle inside ro (see
Fig 2) changes, we need to rewire large portion of the tree which is
done by: 1) Rewiring a random part of the tree (Algorithm 4); 2)
Rewiring starting from the tree root (Algorithm 5).
Lines 3-7 and lines 5-9 of Algorithms 4 and 5 rewire neighbor nodes
(Xnear) of xr and xs, respectively. Thus, rewiring is done in xnear if
by changing its current parent to xj, ci for xnear reduces where xj is
xr and xs in Algorithms 4 and 5, respectively. The difference between these algorithms is the focus point of the rewiring. Algorithm
4 rewires random part of the tree starting from nodes around xrand
or xclosest that are added to Qr in Algorithm 2. Then if rewiring happens to any xnear, Algorithm 4 adds xnear to Qr since nodes around
xnear have the potential to get rewired (line 8). However, Algorithm
5 focuses rewiring the tree around x0 and thus around the agent.
Hence, it starts rewiring from x0 (line 3) and pushes all its neighbor
nodes to Qs. Then, it continues to push nodes with greater distance
from x0 by popping nodes (xs) from Qs (line 5) and pushing xnear
around xs (line 11) when the condition in line 10 is met. Using Qs
allows us to rewire the neighbor nodes with the same ci values from
x0. We refer to the nodes with the same cis as Rewiring Circle from
x0 (see crossed points in Fig 1). Rewiring continues at each iteration until the condition in lines 9 and 12 of Algorithms 4 and 5 is
met, respectively. If time is up and there are still nodes in Qr or Qs
that should get rewired, we can continue rewiring in the upcoming
iterations.
By pushing nodes with the potential need of rewiring their neighbors to Qr, Algorithm 4 intensifies the effect of random sampling
in line 2 of Algorithm 2. Also, by the combination of using Qr
for random rewiring with focusing sampling inside the Ellipsis that


Contents
Abstract
Introduction
Simplified model and distance estimating
An improved collision-free algorithm based on VPF
Simulations and experimentation
Conclusions
Declaration of conflicting interests
Funding
ORCID iD
References
PDF / ePub
Cite article
Share options
Information, rights and permissions
Metrics and citations
Figures and tables
Abstract
This article presents a non-collision trajectory planning algorithm in three-dimensional space based on velocity potential field for robotic manipulators, which can be applied to collision avoidance among serial industrial robots and obstacles, and path optimization in multi-robot collaborative operation. The algorithm is achieved by planning joint velocities of manipulators based on attractive, repulsive, and tangential velocity of velocity potential field. To avoid oscillating at goal point, a saturated function is suggested to the attractive velocity potential field that slows down to the goal progressively. In repulsive velocity potential field, a spring damping system is designed to eliminate the chattering phenomenon near obstacles. Moreover, a fuzzy logic approach is used to optimize the spring damping coefficients for different velocities of manipulators. Different from the usual tangential velocity perpendicular to the repulsive velocity vector for avoiding the local minima problem, an innovative tangential velocity potential field is introduced that is considering the relative position and moving direction of obstacles for minimum avoidance path in three-dimensional space. In addition, a path priority strategy of collision avoidance is taken into account for better performance and higher efficiency when multi-robots cooperation is scheduled. The improvements for local minima and oscillation are verified by simulations in MATLAB. The adaptabilities of the algorithm in different velocities and priority strategies are demonstrated by simulations of two ABB robots in Robot Studio. The method is further implemented in an experimental platform with a SCARA and an ABB robot cooperation around a stationary obstacle and a moving object, and the result shows real time and effectiveness of the algorithms.
Introduction
To meet the requirements of flexible and agile manufacture, multi-robot cooperation that enables the completion of tasks faster, cheaper, and more reliable is drawing more attention. However, the working environment as well as tasks for robots will be more complex and diverse. Therefore, it becomes particularly important in many applications for robots in avoiding collision with each other and other objects, such as cooperative assembly,1 coordinated handling,2 and remote monitoring.3
Currently, there are lots of studies on robots collision avoidance mainly with the following solution.
(1) A* algorithm
A* algorithm is one of the best known path planning algorithms, which can be applied on metric or topological configuration space. This algorithm uses a combination of heuristic searching and searching based on the shortest path. However, these results were not entirely satisfactory and good, and the computational time was very large.4 So some modifications of A* algorithm or accelerated search strategy are suggested.4–7
(2) Configuration space
The configuration space or C-space in robot path planning is defined as the set of all possible configurations of a robot. C-space is N-dimensional where N is the number of degrees of freedoms of the robot.8 Each point in it uniquely represents one position of the robot, for example, the position and orientation of a mobile robot or the joint angles of an industrial manipulator.9 This method is generally utilized to avoid stationary obstacles.10
(3) Grid modeling
Grid method uses grid to be units of environmental information during path planning, and environment is quantified as grid having a certain resolution. It can avoid the complex calculations when dealing with the boundaries of the obstacles.11 This method can be applied to path planning in three-dimensional (3-D) environment.12 The size of the grid has a direct impact on the length of the planning and precision, so it will also take up a lot of storage space if the working area is large and planning path is very accurate.13
(4) Probabilistic road maps (PRMs) and rapidly-exploring random trees (RRTs)
The most well-known sampling-based algorithms include PRM and RRT. However, PRMs tend to be inefficient when obstacle geometry is not known beforehand.14 RRTs have the advantage that they expand very fast and bias toward unexplored regions of the configuration space and so covering a wide area in a reduced time.15 Although the RRT algorithm quickly produces candidate feasible solutions, it tends to converge to a solution that is far from optimal.16
(5) Artificial potential field
In collision-free algorithms, the artificial potential field (APF) method firstly introduced by Khatib17 is particularly attractive due to its simplicity and efficiency. The basic concept of APF is that robot moves from a starting point to a goal point with the action of attractive potential function and repulses away from the obstacles with the action of repulsive potential function. The establishment of APF is simple and fast, which makes it popular for collision-free path planning of mobile robots and articulated manipulators.
For the original APF, robot under the action of virtual force moves from the starting point to the goal point without colliding with obstacles.18–21 So the method is also called force potential field (FPF).
However, Barbara22 presents that the FPF method based on the force or torque mode is difficult to carry out on practical industrial robot. Jaryani23 proposes the concept of velocity potential field (VPF). Unlike the traditional force control approach, VPF transforms distances into the velocity of robot and has no concern of dynamic issues. The attractive velocity is assigned the vector of making the robot head toward an identified goal. In contrary to the attractive field, the repulsive velocity has inverse effect on the robot and it generates a differential motion vector that points the robot backward to the obstacle.
APF method may suffer from local minima problem when attractive and repulsive forces/velocities confront each other on the same line. Many solutions, such as using virtual target24–26 or introducing a tangential potential field,19,27 have been put forward on it. To bypass obstacles, a tangential potential field is usually applied normal to the repulsive potential field.
VPF is a simple and direct method and is applied in collision-free trajectory planning of mobile robots, articulated robot manipulators, and unmanned aerial vehicle.
Kitts28 defines the attractive velocity as a constant from the current point to the target point. The repulsive field includes a repulsive translational velocity and an angular velocity. The repulsive velocity magnitude may be a constant or changes in strength with an inverse proportion of the r-th power of distance between the robot and obstacle, where r is greater than or equal to one. The angular velocity component can solve the local minima problem. To make sure robots arrive at the target quickly, the attractive function usually sets a higher speed. When approaching the goal, the robot may pass the goal in the next periodic interval with the speed that may result in oscillation near the goal. Huang29 defines that attractive velocity is proportional to the distance between the robot and the target. For the repulsive velocity, a radius of the influence of the repulsive field is suggested. Within the circle of influence, the magnitude of the vector grows from zero (outside or on the edge of the circle of influence) to a speed in inverse proportion to the distance between the robot and the obstacle. Jaryani23 introduces a radius of influence of the attractive field, outside of this circle of extent, the vector magnitude keeps a constant and higher value. Within this circle of extent, the vector magnitude is set proportional to the distance between the robot and the goal. When the robot reaches the goal, the attractive velocity is reduced to zero. The repulsive potential has the same definition as in the study by Kitts et al.29
The abovementioned studies are VPF-based collision avoidance happened in two-dimensional (2D) space.
Hu30 proposes a method based on velocity vector field for unmanned aerial vehicle path planning. Actually, the path is planned in 2-D space for the projection of terrain. Attractive velocity function is defined as a constant speed. The repulsive velocity is inversely proportional to the square of the distance between planning points and threat points. The tangential velocity has the same value as the repulsive velocity, whose direction is perpendicular to the repulsive velocity pointing to the side of the target point. OuYang and Zhang31 have a study on collision avoidance with industrial manipulators. Attractive velocity function is defined as a constant speed. An approaching velocity of the manipulator and the obstacle is introduced for repulsive velocity function (RVF). The repulsive velocity is active when the distance between the manipulator and the obstacle falls within the circle of influence and the approaching velocity is less than zero, which indicates the manipulator is approaching the obstacle. The smaller the distance and/or the approaching velocity is, the higher the repulsive velocity is. A detour velocity is deployed to escape local minimal here. The vector of detour velocity is calculated by the vector cross-product without the consideration of obstacle relative position to the robot or moving direction. Wu32 studies the method to grasp a static or moving object with collision avoidance in dynamic environments. Attractive vector contains the translation and rotation information to approach the target. The translation part has a linear relationship with the distance between the end-effector and the target, the rotation part relates to the rotation of the target. Repulsive vector magnitude is evaluated using the exponential of robot–obstacle distances, and the direction relies on all obstacle points. According to the final attractive vector and repulsive vector, the local minima escaping strategy is determined to apply or not. The potential vector is generated by attractive and repulsive vectors that are different in various cases.
In this article, a more applicable algorithm is proposed based on VPF in 3-D working space of robot collaboration around obstacles. Based on the form in the study by Jaryani,23 VPF is improved by introducing a saturation function at attractive VPF and designing a fuzzy spring damping system in repulsive VPF for a more smooth and time-efficient trajectory. Furthermore, an innovative tangential VPF is proposed for minimum avoidance path in 3-D space that is considering more factors, such as the current position relations of obstacles, target, and robots, and moving direction of the obstacle. In addition, a path priority strategy of collision avoidance is introduced for better performance and higher efficiency when multi-robots cooperation is scheduled.
The architecture of robots collaboration with collision avoidance is shown in Figure 1.


3.1. Simulation of Improved Velocity Potential Field Method and Comparative Analysis
3.1.1. Obstacle-Free Simulation Verification
Assuming that the maximum speed of the manipulator is within its speed limit, the trajectory of the manipulator from the initial position to the target position is shown in Figure 6a. Set the maximum attractive velocity boundary radius to 𝑑max=0.35, the attractive velocity dividing factor to 𝑑1=0.15, and the intensity coefficient of attraction speed to 𝛼=0.15, with a potential field-based step planning interval of 1 s. Set the range parameters of the repulsive potential field to 𝑑𝑓=0.15 and to 𝑑𝑠=0.1. Set the joint angles of the initial and target points of the robot arm as shown below:


B
^
VB
VOB
A
^
RVA
V
A
Sf
S
d
S
r
Figure 11: General structure of the reachable avoidance velocities.
absolute velocities generating trajectories tangent to Bb
, since their corresponding relative
velocities lay on f and r . For example, the only tangent velocities in Figure 7 are
represented by the segments KH and LM of the reachable avoidance velocity set RAV.
Since the tangent velocities are uniquely determined by f and r, we use them
to label the subsets in RAV containing velocities generating a single type of avoidance
maneuver. The set RAV in Figure 11 is subdivided into the three subsets Sf , Sr , and
Sd by the boundary of V O and by the straight line passing through Ab
and the apex of
V OB. Each of these subsets corresponds to rear, front, or diverging avoidance maneuvers,
respectively, as stated in the following Lemma. A diverging maneuver is one that takes
the robot A away from the obstacle B.
Lemma 2: The reachable avoidance velocities RAV due to a single obstacle consists
of at most three non-overlapping subsets Sf , Sr , and Sd, each representing velocities
corresponding to front, rear or diverging maneuvers, respectively. 2
The proof of this Lemma is given in Appendix A.
The RAV set due to one obstacle may consist of at most three subsets. Clearly,
there can be cases where RAV consists of fewer sets, one, two or none, as was shown in
Figure 7. For the case of multiple obstacles, we represent each RAV set by Ss, where s is
an ordered string of characters (f,r,d) representing the type of avoidance of each obstacle.
For example, the set Sf f shown in Figure 12 includes velocities generating maneuvers that
avoid both obstacles B1 and B2 with a front maneuver. The set Sf r corresponds to the
front avoidance of B1 and rear avoidance of B2, respectively.
12
VOB


Figure 18. Trajectories of the two robots with different priorities.
Experiment
In this experiment, robot ABB (IRB120) and SCARA working in an overlapped working envelope are set up. The collision avoidance algorithm is implemented on two robots with two obstacles of which one is stationary and the other is moving. The base coordinate of SCARA is defined as global coordinate frame. ABB robot moves from A(480, 240, 200) to B(380, −240, 200) at a speed of 10 mm/s. Starting point and goal point of SCARA are C(100, 400, 100) and D(400, 0, 100) with a speed of 25 mm/s. Static obstacle represented by a LSS (radius: 40, length: 180) is located at (200, 180, 50). An object staying on a moving belt at a speed of 20 mm/s which is represented by a LSS (radius: 70, length: 200) can be considered as a dynamic obstacle. In this scenario, SCARA and ABB robot have the same priority level.
Figure 19 records the behaviors of collision avoidance in different time intervals. The trajectories of the two robots and a moving obstacle at the projection of XY plane are shown in Figure 20, which are denoted in the same color at the same time. Figure 19(a) is in the initial state. Figure 19(b) shows the two robots are moving toward their own goal without obstacles. Figure 19(c) and (d) shows the process of SCARA avoiding the static obstacle and ABB robot avoiding the dynamic obstacle, which correspond to ① and ② in Figure 20(a). When t = 25 s as shown in Figure 19(e), SCARA robot almost arrives to the goal D and ABB robot prepares to avoid SCARA. Figure 19(f) and (g) show that the two robots are avoiding collision with each other, which correspond to ③ in Figure 20(a). As shown in Figure 19(h), the two robots are succeeded in reaching their own goals without any collisions. From Figure 20(b), which magnifies the trajectories in Figure 20(a), it can be found that the trajectories are very smooth when the robots enter or leave repulsive potential field. Experiment results show the two robots successfully avoid collision with each other and the two obstacles.


3.2. Manipulator Real-Time Obstacle-Avoidance Planning Experiment
3.2.1. Experiment Initialization Settings
This paper’s experiments were conducted with a KINOVA Mico2 lightweight manipulator (Kinova Inc., Boisbriand QC, Canada) and a Kinect 2.0 vision sensor (Microsoft, Redmond, WA, USA). The primary hardware of the computer comprises an Intel i5-4590 CPU (Intel, Santa Clara, CA, USA) and an AMD HD-6500 graphics card (AMD, Santa Clara, CA, USA), and the system usesVS2015 software with OpenCV3.4.1, Kinect, and related libraries. We collect human body data and display the recognition of human body joint points, along with storing the data in C++. We simulate the manipulator using the Robotics System Toolbox in Matlab2020b, and the data is transferred to the manipulator for experiments. Figure 10 illustrates the experimental platform that was assembled in this paper.


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g010.jpg
Figure 10
The experimental platform.


𝜆(𝑥,𝑦,𝑧)=∣∣𝑣𝑎𝑡𝑡∣∣∣∣𝑣𝑎𝑡𝑡𝑥∣∣+∣∣𝑣𝑎𝑡𝑡𝑦∣∣+∣∣𝑣𝑎𝑡𝑡𝑧∣∣
(1)
The distance separating the end of the manipulator from the target point determines three distinct zones: acceleration section [𝑑0,𝑑𝑚𝑎𝑥], uniform speed section [𝑑𝑚𝑎𝑥,𝑑1], and deceleration section [𝑑1,0]. For the x-direction, the attraction velocity function can be defined as:


Robot path planning is a crucial research domain in robotics, as robots can perform a plethora of tasks efficiently, via both offline teaching and online autonomous planning, without depending on path-planning technologies. However, as robots move towards collaborating with humans, including in domestic scenarios, the necessity for local path planning is accentuated. The necessity for path planning has, therefore, become a vital research area in robotics. Common path-planning algorithms include A* [13,14], PRM [15,16], fast extended random tree (RRT) [17,18], and APF [19,20,21]. APF guides the manipulator to reach the target or circumvent obstacles through virtual potential fields, force, or velocity potential fields. The artificial potential field (APF) algorithm is a classical method for robot trajectory planning, which is characterized by good real-time performance and minimal computational consumption. Hence, numerous academics have improved the APF algorithm. To accomplish joint-limit and obstacle avoidance, Ye et al. [22] integrated the quadratic programming-based control strategy with the APF approach. Yang et al. [23] modified the action range of the gravitational field and the direction of the original algorithm. When the UAV is caught in a local minimum, a rectangular model centered around the initial target and the corner point is selected as a temporary target based on the evaluation function, thereby the local minima problem is solved. The artificial velocity potential field approach was utilized by Christoff et al. [24] to execute the movements and trajectory planning of two 6-DoF surgical manipulators. Min et al. [25] proposed an experience-based modified potential field method that uses previous experience to achieve obstacle avoidance when the type of obstacle is identified as a known type, or using the modified artificial potential field method otherwise.


Help
Accessibility
Careers


As the position of link 1 is fixed, the distance between the hand and link 1 remains unchanged after the hand trajectory reaches the target position. According to the curve information in Figure 15c, it can be seen that during the motion process of the manipulator, the distance between the end link 6 of the manipulator and the hand does not enter the set safety threshold of 0.15 m. Obstacle-avoidance planning begins after predicting the oscillation by three steps, so the minimum distance is close to 0.15 m but does not breach it before obstacle-avoidance planning starts.


B. Generating the Set of Avoidance Velocities
The procedure used for generating the sets of Reachable Avoidance Velocity RAV,
Si (i=d,f,r), for robot A is illustrated in Figure 24. The Figure shows the set of reachable
velocities RV, the velocity obstacle V OB for a long time horizon Th, and the line laa
passing through Ab
and the apex of V OB. The Si sets are represented by the ordered
lists of their vertices Vi = f(xi ; yi); (l
i1; l
i)g, each specied by its position and by its
supporting lines. The lists are ordered counter-clockwise so that the interior of the set is
on the left of its boundary.
The procedure for computing Si consists of the following steps:
1. Compute the sets of diverging velocities and of non-diverging velocities by intersecting RV with the line laa. Figure 24-a shows the two sets represented by vertices
(5; 2; 3; 6) and(1; 5; 6; 4) respectively. Label the set of diverging velocities Sd and the
set of non-diverging velocities Snd.
2. Compute the intersections between the boundary of Snd and of the velocity obstacle
V OB, and insert them into the list representing Snd. Figure 24-b shows the intersections as points (7; 8; 9). The new list representing Snd becomes (1; 5; 8; 6; 4; 9; 7)
27
3. Compute the sets of non-colliding velocities, i.e. (Snd 	V OB) by marching counterclockwise along the boundary of Snd and clockwise along the boundary of V OB,
starting from the rst vertex that is external to V OB. This corresponds to assigning
a positive value to Snd and a negative value to V OB. Figure 24-c shows schematically
the march along the boundaries of Sf , starting from vertex 1 and generating the
dierence sets represented by (9; 8; 6; 4) and by (1; 5; 8; 7).
4. Label the non-colliding velocity sets according to the segments of V OB included in
their boundary. The set including a segment of the rear boundary of V OB r , is
the set of rear avoidance velocities Sr , and the set including a segment of the front
boundary f is the set of front avoidance velocities Sf , as shown in Figure 24-d.
To consider multiple obstacles, this procedure is applied recursively to each subset Si,
(i=d,f,r), of RAV, generating smaller subsets Ss, where the index s is a string representing
the type and sequence of avoidance maneuvers of each obstacle. For example, three
obstacles might generate up to 9 avoidance velocity sets, such as Sf rd representing a front
avoidance maneuver of the rst obstacle, a rear avoidance of the second, and a diverging
maneuver from the third obstacle.
The upper bound on the number of sets Ss is 3m, where m is the number of obstacles
in the environment. However, the actual number is much smaller, because the lines laa
from A to the apex of each V OBj do not intersect each other. Also, many of the potential
subsets Ss may be covered by some of the V OBj
. Generally, we observed that the higher
the number of obstacles, the fewer the sets Ss remaining to be computed. To ensure that
the set RAV is not empty, sometimes it is necessary to shorten the time horizon, thus


In Figure 5, points 𝑂1−𝑂3 denote the end coordinates of the manipulator upon entering the oscillation region, the center point coordinates of the obstacle and the target point coordinates, respectively. As the three points do not fall on the same straight line, they determine a plane, designated as 𝑄0. This plane serves as the tangent of the sphere in the repulsion potential field, as seen in Figure 5. Connecting the points 𝑂1𝑂2 and the outer circle intersect at points 𝑎1 and 𝑎2. A tangential plane 𝑄1 is formed, which is perpendicular to the plane 𝑄0. Connect 𝑂1𝑂2 and the outer circle to intersect at point 𝑎1,𝑎2, and make a tangent plane perpendicular to the plane of 𝑄0 over 𝑎1, defined as 𝑄1. Make a tangent plane to the outer circle over point 𝑂3, and define the tangent plane where 𝑏1 is located as 𝑄2. The intersection point of 𝑄0, 𝑄1, and 𝑄2 is the virtual target point. The coordinates of the virtual target point G can be determined by correlating the three-plane equations. The manipulator moves to the virtual target point G from its initial position of oscillation. Once reaching the virtual target point, the manipulator moves straight towards the real target point. In this process, the path planning of the manipulator will be re-run. The virtual target point will be used as the starting point for re-planning, the original target point will be used as the termination point for planning, and the path planning will be performed using the improved VPF algorithm. The manipulator will continue to move towards the original target point.


3.2.2. Collision-Free Scenario Path-Planning Experiments
The path-planning condition of the manipulator without the possibility of the collision was modeled and tested. The human right-hand was utilized as the experimental object, with the activity carried out inside a particular range outside the safety radius of the manipulator. The position of the right-hand joint point was obtained in real-time by a camera, and the distance between the right-hand joint point and each linkage of the manipulator envelope cylinder was determined by the control program. The real-time speed of the manipulator was also solved. As the human joint position was always ensured in the manipulator avoidance trigger position, which is beyond the range of the repulsive potential field, the simulation speed was delivered straight to the manipulator for the experiment. This was done to test the execution speed of the manipulator when no collision happens and to compare it with the simulation speed. During the operation of the manipulator, the information of the human right-hand joint is captured in real-time, as illustrated in Figure 11a, which depicts the left-hand due to the mirror image of the Kinect 2.0 camera. Figure 11b shows the real-time distances between the position of the right-hand joint and each linkage of the manipulator envelope cylinder.


In Figure 12a, the blue sample points in the simulation environment represent the movement trajectory of the human right-hand, which does not affect the movement of the manipulator, and allows it to reach the target position accurately. The range of the repulsive potential field is set to be 0.15 m in the simulation experiment. In the actual operation process, the distance between the manipulator and the arm joint has not reached the set repulsive distance. At the same time, the safety distance between the manipulator and the human body is set to be 0.15 m. Therefore, the distance between the manipulator and the arm never breaks through the safety distance during the running process. The whole experimental process is very safe, and the manipulator accurately reaches the target position. Figure 12b demonstrates that the final target pose of the manipulator in both the simulation and experimental environments is consistent. The speeds of the manipulator were collected at intervals of 0.5 s in both the simulation and experimental environments and are shown in Figure 13.


An external file that holds a picture, illustration, etc.
Object name is sensors-23-09617-g015.jpg
Figure 15
The results of the manipulator running. (a) Obstacle-avoidance angular of the manipulator; (b) Obstacle-avoidance angular velocity of the manipulator; (c) Distance between the manipulator and the obstacle in real-time obstacle avoidance.


Google Scholar
18. Ding XQ. Vision trajectory planning for mobile robots based on hybrid artificial potential field. J Zhejiang Univ Sci B 2016; 50: 1298–1306.
GO TO REFERENCE
Google Scholar
19. Masoud AA. Decentralized self-organizing potential field-based control for individually motivated mobile agents in a cluttered environment: a vector-harmonic potential field approach. IEEE Trans Syst Man Cybern A Sys Hum 2007; 37: 372–390.
GO TO REFERENCE
Crossref
Google Scholar
20. Xing Y. A new method for robot path planning based artificial potential field. In: 2016 11th IEEE conference on industrial electronics and applications, Hefei, China, 5–7 June 2016, pp. 1294–1299. IEEE.
Google Scholar
21. Wang H, Lyu W, Yao P, et al. Three-dimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system. Chin J Aero 2015; 28(1): 229–239.
GO TO REFERENCE
Crossref
Google Scholar
22. Barbara MB. A framework for implementing cooperative motion on industrial controllers. IEEE Trans Robot Auto 2004; 20(3): 583–589.
GO TO REFERENCE
Crossref
Google Scholar
23. Jaryani MH. An effective manipulator trajectory planning with obstacles using virtual potential field method. In: 2007 IEEE international conference on systems, man, and cybernetics, Montreal, QC, Canada, 7–10 October 2007, pp. 1573–1578. IEEE.


RESOURCES
Similar articles
Cited by other articles
Links to NCBI Databases
FOLLOW NCBI
Connect with NLM


(7)
where

,

,

; φ, θ, and ψ are the robot’s Euler angles and φG, θG, ψG are the robot’s final Euler angles.
In most cases, the Jacobian matrix is not a square matrix when translating velocity in Cartesian space into velocity in joint space. Therefore, pseudoinverse Jacobian matrix piv(J) needs to be constructed, which is defined as
(8)
Using the inverse Jacobian relationship, we will have the joint velocities

(9)
where J is Jacobian matrix,

 and n is the number of robot joints.
RVF based on fuzzy spring damping system
As the same reason of the oscillation near the goal, the trajectory of the robot at the edge of RVF will be rough. In order to make the robot have a smooth trajectory when it repels the edge of repulsive VPF, a virtual spring damping system is introduced into RVF, whose coefficients are kp and kd around an obstacle as shown in Figure 6; the system generates a repulsive velocity vr when robot enters into the repulsive potential field. No matter the robot entering or leaving repulsive potential field, the damping system will “dissipate” the robot’s energy (velocity). Affected by damping, the oscillation of the robot will be reduced near the obstacle.