https://orcid.org/0009-0009-1780-0024
Figures
Abstract
Ensuring that a robot employing demonstration learning models can simultaneously achieve accurate trajectory tracking of demonstrated paths and effective avoidance of moving obstacles in dynamic environments remains a critical research challenge. This paper proposes a real-time trajectory planning framework based on an enhanced artificial potential field (APF) approach to address this dual-objective problem. Specifically, the proposed method deploys a sequence of virtual target points along the demonstrated trajectory to guarantee both path-following precision and goal convergence for robotic systems. A dynamic obstacle repulsion model is developed by integrating velocity-coupled and acceleration-associated force components, enabling proactive obstacle motion anticipation and adaptive trajectory reconfiguration. Furthermore, a probabilistic obstacle motion prediction framework is established through motion pattern analysis to actively optimize the robot’s motion strategy and reduce tracking errors. Simulation-based experimental results demonstrate that, under complex obstacle motion scenarios, the proposed method achieves a 55.8% reduction in trajectory tracking error compared with recently proposed improved APF methods and a 41.5% decrease relative to Dynamic Movement Primitives (DMP) baselines. These quantitative improvements validate the framework’s superior robustness and safety performance in unstructured environments, with all evaluations systematically conducted in simulated settings.
Citation: Di L, Huang N, He J, Wu X, Huang H, Su Y, et al. (2025) Trajectory tracking and obstacle avoidance in dynamic environments using an improved artificial potential field method. PLoS One 20(7): e0326879. https://doi.org/10.1371/journal.pone.0326879
Editor: Zeashan Hameed Khan, King Fahd University of Petroleum & Minerals, SAUDI ARABIA
Received: January 22, 2025; Accepted: June 5, 2025; Published: July 10, 2025
Copyright: © 2025 Di et al. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited.
Data Availability: All relevant data are within the manuscript and its Supporting information files.
Funding: The author(s) received no specific funding for this work.
Competing interests: The authors have declared that no competing interests exist.
Introduction
The application of robots in industrial scenarios has become increasingly widespread, allowing them to replace humans in performing hazardous, labor-intensive, and repetitive tasks. Trajectory modeling methods for robots based on demonstration learning have gained significant attention due to their ability to simplify trajectory programming and their strong generalization capability. However, in real industrial environments, the working conditions of robots are often highly dynamic [1]. While demonstration learning models can handle changing target points and generalize trajectories while preserving their key features, they are not robust to disruptions caused by obstacles along the way. Moreover, real-world scenarios often involve not only static obstacles but also dynamic ones, which further complicates the application of demonstration learning models. Consequently, how to effectively achieve trajectory tracking or reconstruction of demonstration learning models in complex and dynamic environments, while maximizing the preservation of demonstrated features and simultaneously avoiding dynamic obstacles, has become a research problem of great importance.
Obstacle avoidance algorithms for robots include Dijkstra’s algorithm [2], genetic algorithms [3], algorithm [4], dynamic window approach (DWA) [5], artificial potential field (APF) method [6], rapidly-exploring random trees (RRT) [7], and neural network-based approaches [8]. Additionally, some algorithms combine multiple methods, such as the integration of genetic algorithms and DWA [9], ant colony optimization with genetic algorithms [10], and
combined with DWA [11,12]. Although these traditional path-planning algorithms provide valuable insights, they still require significant improvement when handling dynamic obstacles. Kobayashi et al. [13] proposed a novel local path-planning method based on DWA and virtual manipulators, generating path candidates by modifying variable speeds and predicting the positions of static and dynamic obstacles. This method produces obstacle-avoidance paths that are non-linear and non-arc-shaped in dynamic environments. Chen et al. [14] proposed a dynamic obstacle avoidance method for robotic arms using the soft actor-critic (SAC) algorithm in deep reinforcement learning. They designed a comprehensive reward function integrating dynamic obstacle avoidance and target proximity, enabling real-time planning in complex environments. Wang et al. [15] developed a hybrid path-planning algorithm combining global and local strategies. They improved particle swarm optimization (PSO) for global path optimization by introducing opposition-based learning (OBL) and refining inertia weights and search steps to avoid premature convergence and enhance stability. Furthermore, Zhang et al. [16] proposed a globally guided reinforcement learning (G2RL) approach that incorporates a novel reward structure, enabling generalization to arbitrary environments. Chang et al. [17] introduced an enhanced DWA based on Q-learning, adding two new evaluation functions and adaptively learning DWA parameters through Q-learning to adapt to unknown dynamic environments. However, these methods focus solely on obstacle avoidance and fail to simultaneously address trajectory tracking and pre-defined path following.
The APF method is widely applied in robot obstacle avoidance and trajectory planning due to its computational simplicity and real-time performance [18–21]. Although the traditional APF method suffers from issues such as local minima and the inability to reach goals near obstacles, these limitations have been effectively addressed in recent studies [22,23]. Regarding the challenge of designing obstacle-avoidance trajectories that can follow a pre-defined path, many researchers have proposed improvements to the APF method. Wang et al. [24] proposed a dynamic target artificial potential field (DTAPF) algorithm, which uses dynamic points along a global path generated by the algorithm as APF target points. Additionally, they introduced an offset guidance method to avoid dynamic obstacles while ensuring that the optimized trajectory closely follows the global path. Sang et al. [25] developed a multi-subgoal artificial potential field (MTAPF) method based on an improved APF approach. The MTAPF algorithm divides the globally optimal path generated by an improved
algorithm into a series of sub-goals and employs the improved APF method between these sub-goals to achieve global path tracking. Li et al. [26] enhanced the traditional APF method by adding virtual target points, enabling unmanned tracked vehicles to avoid obstacles and reach target points in off-road environments. They also developed an improved dynamic obstacle avoidance model incorporating relative velocity and acceleration functions to enhance the smoothness of lane-changing obstacle avoidance paths. While these improvements partially address trajectory tracking and dynamic obstacle avoidance, they lack modeling and prediction of obstacle motion, resulting in poor trajectory quality in scenarios with non-uniform and unpredictable obstacle movements.
To address the challenges of trajectory tracking and obstacle avoidance in dynamic environments, this paper proposes a trajectory planning method based on an improved APF approach. The key novelties of this work include:
- To address the inherent limitations of traditional APF methods in trajectory tracking tasks, this paper proposes a dynamic target APF framework by strategically deploying a sequence of virtual target points along the demonstrated trajectory. This approach ensures high-fidelity path generation with minimal deviation from the reference demonstration.
- For dynamic obstacle avoidance, we develop an enhanced repulsion model that incorporates both velocity-dependent and acceleration-associated force components. This formulation enables the robot to proactively anticipate obstacle motion patterns and dynamically adjust its navigation direction for collision-free path planning.
- A probabilistic motion prediction model is further established through statistical analysis of obstacle movement patterns. By leveraging historical trajectory data of dynamic obstacles, the framework allows the robotic system to adaptively optimize its motion strategy in real time, thereby enhancing trajectory quality and robustness in complex environments.
The remainder of this paper is organized as follows: The Related Works section reviews related works on dynamic obstacle avoidance. The Proposed Method section introduces the proposed dynamic target APF method, detailing the virtual target point deployment strategy, the enhanced repulsion model with velocity- and acceleration-associated forces, and the probabilistic motion prediction approach. The Simulations section presents the simulation experimental setup and results analysis, including environment configurations, evaluation metrics, and comparative performance against baseline methods. The Conclusion section concludes the paper by summarizing the key contributions and discussing implications for future improvements.
Related works
Previous research on trajectory planning for robots under dynamic obstacle interference has mainly focused on dynamic obstacle avoidance in unmanned surface vessels (USVs), mobile robots, and robotic manipulators. The relevant techniques span various fields, including robotics, control theory, machine learning, and sensor technology.
Dynamic obstacle avoidance research for USVs
Wang et al. [27] proposed a USV autonomous dynamic obstacle avoidance method based on an enhanced velocity obstacle (VO) approach to achieve path replanning. By redefining the geometric obstacle models in conventional VO methods, they introduced a specialized triangular obstacle geometry to reconstruct the velocity obstacle region. Using previously collected data, they fitted and predicted collision times based on detected obstacle distances, azimuths, and other related data, determining the timing of obstacle avoidance to enable accurate dynamic obstacle avoidance in various environments. Zhu et al. [28] introduced a novel Optimal Collision Avoidance Point (OCAP) method for USVs, which integrates a two-degree-of-freedom model for USV dynamics with a velocity obstacle method for obstacle detection and avoidance. This approach enables real-time adaptation to dynamic and complex environments, making it particularly suitable for USV operations in areas with high traffic density or unpredictable obstacles. The resulting algorithm optimizes USV maneuverability for collision avoidance and can handle multiple moving obstacles simultaneously. Wang et al. [29] proposed a Proactive Velocity Obstacle (PVO) approach that preemptively assesses collision risks between USVs and obstacle vessels based on motion state predictions using the mathematical model of USV dynamics. The PVO method was integrated with the Line-of-Sight (LOS) algorithm into a Set-Based Guidance (SBG) framework, creating a Dynamic Collision Avoidance (DCA) solution for USVs. Simulations demonstrated that this solution enabled USVs to avoid small velocity changes while making safer decisions compared to the original SBG framework in multi-vessel scenarios. To address the challenge of detecting and avoiding previously undetected moving obstacles along a globally planned path, Xia et al. [30] proposed a method combining Gaussian Mixture Models (GMM) and Gaussian Process Regression (GPR) to predict the motion of moving obstacles and estimate prediction uncertainties. They further introduced a Nonlinear Finite-Time Velocity Obstacle (NLFVO) method to analyze USV velocities and uncertain velocity vectors of moving obstacles, enabling collision-free velocity selection while minimizing a defined objective function.
Dynamic obstacle avoidance research on mobile robots
To overcome the limitations of the traditional algorithm, such as excessive turning points, redundant nodes, long computation times, and lack of dynamic obstacle avoidance, Li et al. [31] developed an optimized
algorithm using adaptive step-length adjustment and cubic Bézier curves to address issues of excessive turning points and computational inefficiency. To handle dynamic obstacles in complex environments, they proposed a hybrid algorithm combining the improved
algorithm with the Dynamic Window Approach (DWA), resolving the inability of
to avoid dynamic obstacles while preventing local optimization traps for mobile robots. To enable mobile robots to detect, separate, and track static and dynamic objects in unknown environments, Zohaib et al. [40] proposed a ROS-based efficient algorithm for constructing dynamic graphs. This method exploits spatio-temporal positions to detect and track moving objects without relying on prior knowledge of their geometric features. Pei et al. [32] presented an enhanced Dyna-Q algorithm, incorporating heuristic search strategies, simulated annealing mechanisms, and reactive navigation principles into Q-learning based on the Dyna architecture. This approach addressed the challenge of mobile robot path planning in unknown environments with static and dynamic obstacles. They also proposed a novel action selection strategy combining an
-greedy policy with a cooling schedule, improving global search performance, convergence properties, and learning efficiency. Shi et al. [33] enhanced the simulated annealing (SA) algorithm by adding an initial path selection method and a deletion operation, proposing an improved dynamic path planning SA algorithm for avoiding moving obstacles. The good initial path selection helped avoid local minima, while the deletion operation significantly reduced computation time, enabling the algorithm’s application in dynamic environments with fixed and moving obstacles. Kobayashi et al. [13] proposed a Dynamic Window Approach with Virtual Manipulators (DWV), a novel candidate path generation method that incorporates non-linear and non-arc paths. DWV generates candidate paths by modifying variable speeds and predicting the positions of static and dynamic obstacles through virtual manipulators. Consequently, DWV produces obstacle-avoiding paths in environments with both static and dynamic obstacles. Additionally, Intelligent Bug Algorithm (IBA) [39] provides a reactive obstacle avoidance strategy, but it lacks trajectory tracking capability in dynamic environments.
Dynamic obstacle avoidance research on robotic arms
For robotic manipulators executing manufacturing tasks, Han et al. [34] proposed a novel method based on distance computation and discrete detection. Using the Gilbert-Johnson-Keerthi (GJK) algorithm, the minimum distance between a manipulator and dynamic obstacles, obtained via a Kinect-V2 camera, is computed in real time. When this distance falls below a safety threshold, an improved discrete collision detection method determines whether the obstacle intersects the manipulator’s global path. If a collision is predicted, a local goal is set, and the manipulator replans a local path. Chen et al. [35] proposed a path planning method for robotic manipulators based on the Soft Actor-Critic (SAC) deep reinforcement learning algorithm. By designing a composite reward function for dynamic obstacle avoidance and target proximity, the method enables real-time planning and avoids moving obstacles in the environment. The adoption of prioritized experience replay (PER) further improves sampling efficiency. Li et al. [36] introduced a hierarchical replanning framework to quickly adjust ongoing trajectories for 7-DoF redundant robotic manipulators to avoid dynamic obstacles in human-robot interaction scenarios. The framework initializes joint-space paths using an improved Rapidly-Exploring Random Tree (RRT) planner, combines local path rerouting with redundancy-based self-motion, and designs an adaptive online trajectory generator to provide smooth motion primitives in real time. Chen et al. [37] developed a two-stage obstacle avoidance path planning method, consisting of collision detection and avoidance path planning. Collision detection is achieved by building point cloud models and testing the intersection of axis-aligned bounding box trees, while avoidance path planning involves optimizing pre-planned global paths using an improved D-star algorithm, reducing turning points and collision probabilities. Real-time path adjustment strategies ensure manipulators’ accessibility and obstacle avoidance in dynamic environments.
In summary, significant progress has been made in dynamic obstacle avoidance research. However, most studies focus solely on avoiding obstacles during robot or agent motion and do not address trajectory tracking. This research focuses on solving the problem of dynamic obstacle avoidance encountered by robots as they advance along demonstrated trajectories. We propose an improved APF method that ensures the robot moves along the demonstrated trajectory as much as possible while effectively avoiding dynamic obstacles. Additionally, a motion prediction model for obstacles is incorporated to predict potential collisions and adjust the robot’s motion strategy, thereby minimizing deviations from the trajectory.
Proposed method
Concepts of APF method
The APF method was first proposed by Khatib in 1986 [38], ingeniously applying the physical principles and laws of potential fields and mechanics to path planning. This method abstracts an agent as a point moving under the influence of a virtual artificial potential field. These potential fields are categorized into two types: attractive fields and repulsive fields. The potential field function treats the target point as an attractive pole and obstacles as repulsive poles. The attractive pole generates an attraction force, which is a function of the distance between the agent and the target point. Conversely, the repulsive poles generate a repulsion force that depends on the distance between the obstacles and the agent. Each obstacle has an influence range, and repulsion forces are applied only when the agent enters this range. The agent’s motion direction is determined by the vector sum of the attraction and repulsion forces, as shown in Fig 1.
represents the influence range of the obstacle, Fatt represents the attractive force exerted by the target point on the robot, Frep represents the repulsive force exerted by the obstacle on the robot, and Fres represents the resultant force acting on the robot.
Gravitational potential field.
In path planning, the gravitational potential field is a concept that simulates the attraction of a target point to a robot. The main purpose of the gravitational potential field is to guide the robot towards the target point and move it in that direction. The gravitational potential field is typically represented by the following potential function:
Here, Uatt(q) is the potential energy of the gravitational field, katt is the gravitational coefficient, q is the current position of the robot, and qtar is the position of the target point.
The force generated by the gravitational potential field can be represented by the gradient:
Repulsive potential field.
The repulsive potential field is designed to prevent collisions between the robot and obstacles. When the robot approaches an obstacle, the repulsive potential field generates a repulsive force to move the robot away from the obstacle or choose a path around it. The potential energy of the repulsive potential field can be represented by the following function:
Here, Urep(q) is the potential energy of the repulsive field, krep is the repulsive coefficient, qobs is the position of the obstacle, and is the range of influence of the obstacle.
The force generated by the repulsive potential field can be represented by the gradient, taking into account the influence range of the obstacle on the robot’s position:
Resultant potential field.
The resultant potential field is the combination of the gravitational potential field and the repulsive potential field, describing the comprehensive force acting on the robot in the entire environment. By combining the gravitational potential field and the repulsive potential field, the robot can find the optimal path between the attraction of the target point and the repulsion of obstacles, achieving effective path planning and obstacle avoidance.
The potential energy of the resultant potential field can be represented as the sum of the gravitational potential field and the repulsive potential field:
Similarly, the force generated by the resultant potential field is the resultant of the forces generated by the gravitational potential field and the repulsive potential field:
Once the resultant force acting on the robot in the potential field is computed, Algorithm 1 is employed to calculate the final optimized trajectory.
Algorithm 1. APF algorithm for path planning.
Trajectory tracking algorithm based on dynamic virtual target points
Calculation of the attraction of virtual target points and the repulsion of static obstacles.
The traditional APF method performs planning across the entire space, and as a result, it cannot effectively achieve trajectory tracking. Therefore, this paper proposes an improved strategy based on dynamic virtual target points. Multiple virtual target points are placed along the demonstration trajectory connecting the start and end points. At each step, a set of three virtual target points applies an attracting force to the robot until it reaches the destination. Since these virtual target points are distributed along the demonstration trajectory, the generated path closely follows the trajectory.
As shown in Fig 2, suppose that at a given moment, the attractor set consisting of three virtual target points provides attraction to the robot, with each virtual target point assigned a specific weight. Let the current position of the robot be X, and the position of the virtual target point Pk be
. The total attraction force acting on the robot at the current moment is then given by:
here, katt1, katt2, and katt3 are the attraction coefficients for the three virtual target points, respectively. Therefore, this method provides the robot with some predictive capability regarding the direction of the trajectory.
The primary objective of implementing virtual target points is to enable robots to maintain alignment with demonstration trajectories as closely as possible. To achieve this, the attraction coefficients can be systematically optimized by calculating the trajectory tracking errors under obstacle-free conditions across various coefficient combinations. Computational intelligence methods such as Genetic Algorithms (GA) and Ant Colony Optimization (ACO) can be employed as optimization frameworks, with the minimization of trajectory tracking errors serving as the principal performance metric for selecting the optimal parameter configurations.
When the demonstration trajectory crosses the obstacle region, in addition to the attractive force exerted on the robot by the attractor set, the obstacle will also exert a repulsive force. Let the influence range of the obstacle be denoted by . The robot will only experience the repulsive force if it is within this range. However, there is a special case where the robot enters the obstacle’s influence range but its motion direction does not point towards the obstacle and it merely passes through. If the robot is still affected by the obstacle in this case, it could lead to unnecessary deviations in the planned trajectory. Therefore, the obstacle’s influence on the robot’s motion can be determined by checking the number of intersection points between the robot’s motion direction and the obstacle’s boundary.
Let the obstacle’s influence radius be , as shown in Fig 3. At time t, the robot’s position is Xt, and its position at the previous time step is Xt−1. A line connecting these two points is drawn, and suppose it intersects the obstacle at points M and N, and intersects the boundary of the obstacle’s influence range at point Q. If points M and N exist, the repulsive force on the robot is considered when analyzing the forces acting on it. Let the distance from the robot to point Q be denoted as d1, and the distance from Q to M be denoted as d2. The magnitude of the repulsive force acting on the robot is defined as:
where kobs is the repulsive force coefficient, and Fatt is calculated according to Equation (7). When , it indicates that the robot has collided with the obstacle, and the repulsive force experienced by the robot is kobs times the attractive force at that point. The direction of Fobs is from the center of the obstacle towards the robot. If no intersection points M and N exist, the repulsive force is zero at that moment.
To avoid the local optimum problem and ensure the robot safely bypasses the obstacle, a tangential force is defined. As shown in Fig 4, assume that the line connecting the current position of the robot to the center of the obstacle intersects the obstacle’s edge at point M. The two opposite tangential vectors at this point are defined as
and
. The direction of
is chosen as the vector whose dot product with the vector
is positive, and its magnitude is a small fixed value.
The number and distribution of virtual target points.
N virtual target points are evenly distributed between the start and end points of the demonstration trajectory, serving as candidate attractors for the robot. At the initial time, are chosen as the attractor set. In the absence of dynamic obstacles, the subsequent attractor set consists of the next three virtual target points, selected sequentially through a sliding window, until the robot reaches the endpoint. Let L denote the length of the demonstration trajectory, and amax represent the robot’s maximum acceleration. The value of N is defined as:
where is a speed adjustment parameter used to regulate the robot’s speed in the algorithm.
Since these virtual target points are distributed along the demonstration trajectory, the generated path will follow the trend of the demonstration trajectory. In the absence of dynamic obstacles, the force acting on the robot at each moment is calculated using Equation (7), and the robot’s position is updated according to the following formula:
Let cA denote the number of virtual target points occupied by obstacle A, as shown in Fig 5. Suppose obstacle A is currently moving with velocity . Two straight lines are drawn along the direction of
at the edges A1 and A2 of the obstacle, intersecting the demonstration trajectory at points M1 and M2. The point closest to M1 is denoted as Pm, and the point closest to M2 is
. The value of cA is calculated using Algorithm 2.
Algorithm 2. Calculation of cA, the number of virtual target points occupied by obstacle A.
Updating method of the attractor set among the virtual target points.
In the absence of obstacles, starting from the initial attractor set at time zero, the attractor set moves one step forward at each time step until the last attractor reaches the goal. Specifically, after time t, the attractor set becomes
. According to the settings in Equation (10), if the next target point exceeds the position of the first attractor, the next target point is set to the position of the first attractor. Therefore, except for the acceleration phase at the initial time step, the subsequent trajectory will coincide with the demonstration trajectory, achieving accurate trajectory tracking.
When static obstacles are present, if the attractor set continues to advance by one point per time step, oscillations may occur near the obstacle for some time [100]. This is because the component of the attractive force in the forward direction may become negative. Therefore, when the first attractor enters the influence range of the obstacle, the update step size of the attractor set is adjusted based on its distance to the center of the obstacle. Two step sizes, s1 and s2, are introduced, with s1>1 and s2<1, as shown in Fig 6. Specifically, when entering the obstacle’s influence range, the update step size is increased to allow the attractor set to quickly reach the center of the obstacle, ensuring that the resultant force on the robot in the forward direction is positive. After reaching the center, the update step size is reduced to wait, preventing tracking errors caused by the robot failing to keep up with the attractor set.
Schematic diagram of the number of virtual target points occupied by the obstacle
Updating method of the attractor set among the virtual target points.
The traditional APF method assumes static obstacles. However, if the position of obstacles can be updated at each time step, the method can also be applied to dynamic obstacles. A significant disadvantage of the traditional APF method is that it does not account for the movement trend of obstacles, making the robot prone to significant path deviations due to the influence of the obstacles, as shown in Fig 7. To address this issue, we introduce the concepts of virtual velocity-related force and acceleration-associated force. For obstacles that satisfy the distance conditions, their velocity and acceleration are applied to the robot in the form of virtual forces, enabling the robot to react in advance to the movement trends of the obstacles.
When a moving obstacle enters the detection range of the robot, assume the current velocity of obstacle A is and its acceleration is
. In this case, the robot will experience a force perpendicular to
, with its direction aligned with the direction of the obstacle’s velocity and acceleration. As shown in Fig 8,
and
represent the virtual forces associated with
and
, respectively.
To reduce the impact of the randomness in the obstacle’s movement, the velocity of the obstacle over the last three time steps is weighted. For obstacle A, the weighted velocity is obtained. Assume that at time t, the position of the obstacle is qobs(A,t), then
is calculated as:
Let represent the current position of the robot, and
represent the coordinates of the first virtual target point ahead of the robot. Define the virtual target vector
, and define a vector
perpendicular to
, as well as an auxiliary vector
. The vector
can be calculated as:
We define a symbolic expression to determine whether the obstacle is approaching or moving away from the virtual target vector. Here,
denotes the sign of the expression in the parentheses, and d(A,t) represents the perpendicular distance from the center of obstacle A to the line containing
.
and d(A,t) are calculated as follows:
where:
Finally, the virtual velocity-related force applied by obstacle A to the robot is calculated as:
where is the velocity-related factor.
Similarly, the acceleration-associated force can be calculated as:
Where ka is the acceleration-associated factor, and is calculated as:
Therefore, the resultant force acting on the robot is obtained by performing vector summation of the forces calculated from Equations (7), (8), (18), and (19), following the vector analysis and summation procedure illustrated in Fig 1. The specific formulation is expressed as follows:
where O denotes the set of all obstacles.
Construction of the obstacle motion prediction model.
By combining the virtual velocity and acceleration associated forces of the obstacles, the current motion trend of the obstacles can be considered as a factor influencing the robot’s motion. The purpose of this method is to predict the movement of obstacles roughly when they are far from the robot, thus avoiding the obstacles as much as possible before they approach the robot. However, this rough prediction does not guarantee that the robot will successfully avoid the obstacles. When the robot enters the influence range of the obstacle, a more accurate prediction of the obstacle’s movement is required. Based on the potential collision scenarios at the next time step, the forward step length of the virtual target points should be adjusted accordingly.
To describe the uncertainty in the movement of dynamic obstacles, probability statistical methods are used. It is assumed that the velocity and motion angle of the obstacle remain constant within each time step , and that the changes in the obstacle’s velocity and angle follow a normal distribution. For
, the expected velocity and motion angle are defined as:
where and
are both normally distributed:
,
.
Assuming the current time is T, and from time step 0, the velocity and angle changes of the obstacle are recorded as and
, the maximum likelihood estimation is used to compute the mean and variance at the current time step:
At each subsequent time step, the most recent data is added, and ,
,
, and
are recalculated according to the above four formulas to ensure continuous accuracy of the estimation. According to the 3
rule of the normal distribution, nearly all random variables are confined within [
,
]. Therefore, the uncertainty in velocity and motion angle at time t = T is estimated based on the data from
.
Define a region Ol that represents the possible area where the center of the obstacle may be located at the next time step, where l represents the multiple of , with values in {1, 2, 3}. For example, if l = 1, then O1 represents a sector with an outer radius of
, an inner radius of
, and an angular range of
, as shown in Fig 9. The smaller the value of l, the higher the probability that the obstacle will appear in Ol at the next time step, i.e.,
. If at the next time step, the expected position of the robot overlaps with the region Ol, it is determined as a potential collision situation and requires further processing.
Define O4 as the region outside O3, as shown in Fig 10. Assuming the current position of the robot is q0, the robot’s position at the next time step q1 can be calculated as:
where is calculated from Equation (7).
The forward step length p for the three virtual target points is determined by evaluating the regions of q0 and q1. Specifically, at the next time step, the virtual target points move from to
. The value of p is determined according to the rules in Table 1, where cA is calculated by Algorithm 2.
Simulations
Definition of tracking error for complex trajectories.
This section introduces a new method to accurately calculate the tracking error for complex trajectories. The tracking error EA(x) between the obstacle-avoidance trajectory A and the reconstructed trajectory T at the sampling point X = x is defined as follows:
Calculation Steps:
1. Assume that all trajectory points of A are , where NA represents the total number of points in trajectory A. Similarly, all trajectory points of T are
, where NT is the total number of points in trajectory T.
2. Resample trajectory A into M equidistant points, where . Resample trajectory T into
equidistant points, where
is a constant.
3. For each point in the resampled trajectory A, find the nearest point on trajectory T in terms of the x-coordinate. Let the x-coordinate of the point in trajectory A be , and find the closest point in trajectory T with
. Check whether
has been used in previous calculations. If it has, select the second closest point as
, and so on, until an unused point nearest to
is found.
4. Suppose the resampled points of trajectory A, , correspond to points in trajectory T,
. Compute the tracking error for each point as:
5. Compute the maximum tracking error and the mean tracking error as follows:
Under this calculation method, due to the exclusion mechanism, the tracking error can be accurately computed even when there are multiple intersection points on the same vertical line X = x in trajectory A. Previous values will not be overwritten. As shown in Fig 11, when two sampling points on the obstacle-avoidance trajectory A (the two red points in the figure) are located on the same vertical line, the tracking error for the first sampling point is calculated with the green point, while the second sampling point calculates the tracking error with the nearest unused point (the black point in the figure).
In addition, the overlap rate between two trajectories is defined as the proportion of sampled points with a tracking error less than
, which is used to evaluate the degree of feature preservation between the two trajectories. Assuming that among M points, the number of points satisfying
is m, the overlap rate is calculated using the following formula:
Obstacle avoidance experiments in static obstacle scenarios.
Simulation experiments were conducted on a Python-based platform to validate the theoretical feasibility of the proposed method. To evaluate its effectiveness in static obstacle scenarios, a comparative experiment was designed using a reconstructed sinusoidal curve trajectory in a dual-obstacle environment. In the domain of demonstration learning, research focusing on simultaneous preservation of demonstration features and obstacle avoidance remains relatively limited. Current mainstream solutions involve modifying the Dynamic Movement Primitives (DMP) algorithm by incorporating coupling terms to adjust trajectories based on obstacle positions and relative velocities, while improved APF approaches also demonstrate capabilities for trajectory tracking and obstacle avoidance. This study therefore adopts both the DMP algorithm with enhanced coupling terms and the improved APF approach as established benchmarks for comparative evaluation. The key simulation parameters in this experiment are set as follows: ,
,
, s1 = 4, s2 = 0.25,
, and ka = 0.6.
Simulation experiments were conducted on a Python-based platform to validate the theoretical feasibility of the proposed method. To evaluate its effectiveness in static obstacle scenarios, a comparative experiment was designed using a reconstructed sinusoidal curve trajectory in a dual-obstacle environment. In the domain of demonstration learning, research focusing on simultaneous preservation of demonstration features and obstacle avoidance remains relatively limited. Current mainstream solutions involve modifying the DMP algorithm by incorporating coupling terms to adjust trajectories based on obstacle positions and relative velocities, while improved APF approaches also demonstrate capabilities for trajectory tracking and obstacle avoidance. This study therefore adopts both the DMP algorithm with enhanced coupling terms and the improved APF approach as established benchmarks for comparative evaluation. The key simulation parameters in this experiment are set as follows: ,
,
,
,
, s1 = 4, s2 = 0.25,
, and ka = 0.6.
A sinusoidal curve is defined as the reconstructed trajectory, with two static obstacles placed near the two peaks of the curve, as shown in Fig 12.
The three methods were applied to plan obstacle avoidance trajectories, with results shown as curves in the upper section of Fig 13. While all methods achieve safe obstacle avoidance, the proposed method’s trajectory demonstrates closer alignment with the reconstructed trajectory near obstacles. This is attributed to the attractor set’s predictive capability combined with obstacle tangential forces, enabling more accurate trajectory trend anticipation. In contrast, the DMP-based approach over-relies on obstacle distance metrics in coupling term design, prioritizing rapid approach and return to the original trajectory. The improved APF method exhibits residual trajectory deviation requiring further optimization.
By defining , the tracking errors relative to the reconstructed trajectory are visualized in the lower portion of Fig 13. Qualitative analysis reveals the proposed method’s superior trajectory tracking performance. Table 2 quantitatively compares tracking errors across methods, showing reduced maximum and average errors alongside increased trajectory overlap rates for the proposed approach. These results validate its advantages in trajectory tracking accuracy and feature preservation.
Obstacle avoidance experiments in uniform linear motion scenarios.
When the obstacle is not stationary, obstacle avoidance for the trajectory becomes more complex. This section first discusses a relatively simple scenario, where the obstacle moves in a uniform linear motion. As shown in Fig 14, the blue trajectory represents the reconstructed trajectory obtained from the demonstration learning model. The obstacle starts from the position shown in the figure and continuously moves upward with a controlled velocity v. Assuming that after a time t, the obstacle reaches the expected collision point. If, in the absence of obstacles, the trajectory obtained through the proposed method reaches the collision point after time t, as illustrated in Fig 15, a collision would occur without addressing the dynamic obstacle.
To validate the effectiveness and superiority of the proposed method, this study adopts the improved DMP algorithm and the improved APF algorithm as comparative benchmarks. The improved DMP algorithm integrates coupling terms with the dynamic obstacle avoidance method proposed in [41], while the improved APF algorithm implements the approach described in [26].
The obstacle avoidance trajectories obtained by the three methods are shown in the upper part of Fig 16. All three methods successfully avoid the obstacle, but the quality of the obstacle avoidance trajectories differs. The DMP-based method incorporates the obstacle’s velocity and distance into the coupling term, causing the trajectory to be influenced by the obstacle from the very beginning, as the distance between the trajectory and the obstacle decreases before reaching the collision point. Although the trajectory deviates from the reconstructed trajectory at the beginning, its general trend remains consistent with the original trajectory. After bypassing the obstacle, as the distance to the obstacle gradually increases, the obstacle’s influence disappears, and the trajectory quickly returns to the reconstructed trajectory, as shown by the purple trajectory in the figure.
The iAPF-based method follows the reconstructed trajectory before entering the obstacle’s influence range. Upon approaching the obstacle, the trajectory direction experiences a sudden change, followed by oscillatory upward motion pushed by the obstacle. After surpassing the obstacle, the trajectory gradually returns to the original trajectory, as depicted by the green trajectory in the figure.
In contrast, the proposed method leverages the velocity correlation force of the obstacle to predict its movement in advance. When approaching the obstacle, the trajectory direction is adjusted smoothly to bypass the obstacle, as shown by the blue trajectory in the figure.
The lower part of Fig 16 shows the tracking error curves between the three obstacle avoidance trajectories and the reconstructed trajectory, while Table 3 presents the specific tracking error data. It can be observed that the proposed method achieves the smallest average tracking error and maximum tracking error. However, the trajectory overlap rate is slightly lower than that of the iAPF-based method. This is because the iAPF-based method lacks a prediction capability and reacts only after entering the obstacle’s influence range, thereby spending more time in trajectory tracking. On the other hand, the prediction capability of the proposed method enables the trajectory to leave the reconstructed trajectory earlier, allowing for smoother obstacle avoidance and better preservation of demonstration features.
Obstacle avoidance experiments in complex obstacle motion scenarios.
This section discusses a more complex scenario in which the obstacle moves along a curved path, intersecting the reconstructed trajectory twice, as shown by the gray trajectory in Fig 17. Assume the obstacle starts with an initial velocity and reaches collision point 1 after a certain period. Simultaneously, the robot starting from the initial point would also reach collision point 1 without the influence of the obstacle, similar to the scenario described in Fig 15. Since the trajectory length increases after avoiding the obstacle, the originally designed collision point 2 will no longer result in a collision. The obstacle’s motion speed needs to be adjusted to meet the collision requirements. After passing collision point 1, the obstacle’s velocity
is modified based on the time required to reach collision point 2 according to different methods, creating a second collision. Under this setup, the obstacle’s motion speed varies, and its trajectory is not an ideal straight line, making it more consistent with real-world operational scenarios.
The iAPF-based and DMP-based methods are used for comparative experiments, recording the trajectory planning performance of obstacles at different positions to evaluate their obstacle avoidance capabilities. Fig 18 shows the obstacle avoidance trajectory generation process using the proposed method. Figures (a) and (c) illustrate the trajectory planning when the obstacle reaches the collision points. As the obstacle is approached, the proposed method anticipates the collision in advance. The velocity correlation force and acceleration correlation force guide the trajectory out of the collision region ahead of time. After avoiding the obstacle, the trajectory is attracted back to the reconstructed trajectory by the attractor set, ensuring it tracks the reconstructed trajectory as closely as possible, as shown in Figures (b) and (d).
The trajectory generation process using the iAPF-based method is shown in Fig 19. Since this method cannot anticipate the trajectory’s motion, it exhibits oscillations when the trajectory enters the obstacle’s influence region, being pulled along by the obstacle, as shown in Figure (a). After the obstacle moves away, the trajectory gradually returns to the demonstration trajectory, as shown in Figure (b). The same occurs during the second collision; the trajectory undergoes a sudden change upon entering the obstacle’s influence region, as shown in Figure (c). Since the trajectory’s motion direction aligns with the obstacle’s motion direction, and the obstacle’s velocity is slower, the obstacle is successfully avoided, as shown in Figure (d).
The trajectory generation process using the DMP-based method is shown in Fig 20. At the beginning, as the obstacle approaches, the trajectory deviates but still maintains the general trend of the demonstration trajectory and successfully avoids the obstacle. The trajectory then transitions smoothly back to the reconstructed trajectory. Upon detecting the obstacle’s approach a second time, the trajectory changes direction and successfully avoids the obstacle.
The final trajectories generated by the three methods and their corresponding tracking errors are shown in Fig 21, with the specific tracking error data provided in Table 4. It can be observed that the proposed method achieves the smallest average trajectory tracking error. The trajectory overlap rate is the same as that of the iAPF-based method and significantly higher than that of the DMP-based method. However, the proposed method’s maximum trajectory tracking error is slightly higher than that of the DMP-based method, as the DMP method can predict the obstacle’s motion direction earlier, allowing for better deviation control. However, this comes at the cost of a reduced trajectory overlap rate. Overall, the proposed method achieves lower trajectory error, better feature preservation, and relatively smoother trajectories.
Ablation experiment on obstacle motion-associated forces
To validate the effectiveness of velocity-associated force and acceleration-associated force in the algorithm, a simulation experiment was conducted in a scenario involving an obstacle with variable velocity. As shown in Fig 22, the obstacle performs acceleration and deceleration motion between positions A(9.3,5.75) and B(9.3,1.75), while maintaining a constant velocity before reaching position A and after leaving position B. During acceleration, the velocity increases from to
with an acceleration of
, while the reverse is true during deceleration.
Three scenarios were designed to evaluate the effectiveness of velocity-associated and acceleration-associated forces in the algorithm, with the specific configurations listed in Table 5. Scenario A includes both velocity-associated and acceleration-associated forces, Scenario C excludes both, and Scenario B includes only the velocity-associated force.
The trajectories and corresponding tracking errors for the three scenarios in the deceleration case are shown in Fig 23. In Scenario C, due to the absence of associated forces, the obstacle cannot be anticipated, and the response only begins when the obstacle enters the influence range. Although the trajectory overlap rate is higher, the deviation is also greater. Compared to Scenario A, Scenario B exhibits larger tracking errors because the acceleration-associated force in Scenario A counteracts part of the velocity-associated force, enabling the obstacle to be avoided with smaller deviations. The specific average and maximum tracking errors, as well as the trajectory overlap rates, are listed in Table 6.
For the acceleration case, the trajectories and corresponding tracking errors for the three scenarios are shown in Fig 24. In Scenario B, due to the absence of the acceleration-associated force, the obstacle’s velocity changes cannot be anticipated, resulting in a collision. Although Scenario C lacks prediction capability, the repulsive force of the obstacle allows it to successfully avoid the obstacle. Scenario A, however, can anticipate the obstacle’s motion and avoid it with smaller deviations. The specific average and maximum tracking errors, as well as the trajectory overlap rates, are listed in Table 7.
Conclusion
In dynamic obstacle environments, enabling robots based on demonstration learning to follow the demonstration trajectory as closely as possible while smoothly avoiding moving obstacles remained an important challenge in robot trajectory planning. This paper proposed a real-time trajectory planning method based on an improved APF method and a dynamic obstacle prediction model, addressing the issue of enabling robots to avoid dynamic obstacles and accurately track demonstration trajectories in unstructured environments.
First, we improved the traditional APF method by adding virtual target points along the predefined trajectory, enabling the robot to closely follow the demonstration trajectory and successfully reach the goal. Second, for dynamic obstacles, we proposed an improved APF method that incorporated velocity-associated and acceleration-associated forces, establishing a dynamic obstacle repulsive force model. This allowed the robot to predict the motion of obstacles and adjust its direction accordingly. Finally, we developed a probabilistic model-based obstacle motion prediction method to optimize the robot’s motion strategy, significantly reducing trajectory tracking errors.
Simulation results demonstrated that compared to traditional DMP-based and iAPF-based methods, the proposed algorithm achieved significant improvements in both trajectory tracking error and trajectory overlap rate.
In conclusion, the proposed trajectory planning method based on the improved APF method and dynamic obstacle prediction model demonstrated excellent performance in
dynamic obstacle avoidance and precise trajectory tracking. It provided an effective solution for real-time trajectory planning for robots in complex environments.
Acknowledgments
This work was supported by the project “Robot Operation Technology Research for Live-line Disconnection/Connection of Drainage Conductors in Substations Based on Robotic Collaboration Techniques”, under Grant 031200KC23120030.
References
- 1. Saleem O, Hamza A, Iqbal J. A fuzzy-immune-regulated single-neuron proportional–integral–derivative control system for robust trajectory tracking in a lawn-mowing robot. Computers. 2024;13(11):301.
- 2. Sundarraj S, Reddy RVK, Basam MB, Lokesh GH, Flammini F, Natarajan R. Route planning for an autonomous robotic vehicle employing a weight-controlled particle swarm-optimized Dijkstra algorithm. IEEE Access. 2023;11:92433–42.
- 3. Li D, Wang L, Cai J. Research on path planning of mobile robot based on improved genetic algorithm. Int J Model Simulat Sci Comput. 2023;14(06):2341030.
- 4. Kabir R, Watanobe Y, Islam MR, Naruse K. Enhanced robot motion block of a-star algorithm for robotic path planning. Sensors (Basel). 2024;24(5):1422. pmid:38474956
- 5. Seder M, Petrovic I. Dynamic window based approach to mobile robot motion control in the presence of moving obstacles. In: Proceedings 2007 IEEE International Conference on Robotics and Automation. 2007. p. 1986–91.
- 6. Rostami SMH, Sangaiah AK, Wang J, Liu X. Obstacle avoidance of mobile robots using modified artificial potential field algorithm. J Wireless Com Network. 2019;2019(1):1–19.
- 7. Ul Islam N, Gul K, Faizullah F, Ullah SS, Syed I. Trajectory optimization and obstacle avoidance of autonomous robot using robust and efficient rapidly exploring random tree. PLoS One. 2024;19(10):e0311179. pmid:39392842
- 8. Xu X, Cao D, Zhou Y, Gao J. Application of neural network algorithm in fault diagnosis of mechanical intelligence. Mech Syst Signal Process. 2020;141:106625.
- 9. Li Y, Zhao J, Chen Z. A robot path planning method based on improved genetic algorithm and improved dynamic window approach. Sustainability. 2023;15(5):4656.
- 10. Ma K, Wang L, Cai J, Li D, Wang A, Tan T. Robot path planning using fusion algorithm of ant colony optimization and genetic algorithm. Int J Model Simul Sci Comput. 2023;14(06):2341032
- 11. Liu Y, Wang C, Wu H, Wei Y. Mobile robot path planning based on kinematically constrained a-star algorithm and DWA fusion algorithm. Mathematics. 2023;11(21):4552.
- 12. Li P, Hao L, Zhao Y, Lu J. Robot obstacle avoidance optimization by A* and DWA fusion algorithm. PLoS One. 2024;19(4):e0302026. pmid:38683853
- 13. Kobayashi M, Motoi N. Local path planning: dynamic window approach with virtual manipulators considering dynamic obstacles. IEEE Access. 2022;10:17018–29.
- 14. Chen P, Pei J, Lu W, Li M. A deep reinforcement learning based method for real-time path planning and dynamic obstacle avoidance. Neurocomputing. 2022;497:64–75.
- 15. Wang Z, Li G, Ren J. Dynamic path planning for unmanned surface vehicle in complex offshore areas based on hybrid algorithm. Comput Commun. 2021;166:49–56.
- 16. Wang B, Liu Z, Li Q, Prorok A. Mobile robot path planning in dynamic environments through globally guided reinforcement learning. IEEE Robot Autom Lett. 2020;5(4):6932–9.
- 17. Chang L, Shan L, Jiang C, Dai Y. Reinforcement based mobile robot path planning with improved dynamic window approach in unknown environment. Auton Robot. 2020;45(1):51–76.
- 18.
Warren C W. Global path planning using artificial potential fields. 1989 IEEE International Conference on Robotics and Automation. IEEE Computer Society, 1989. p. 316–21.
- 19. Li Y, Tian B, Yang Y. Path planning of robot based on artificial potential field method. In: 2022 IEEE 6th Information Technology and Mechatronics Engineering Conference (ITOEC). 2022. p. 91–4.
- 20. Duan Y, Yang C, Zhu J, Meng Y, Liu X. Active obstacle avoidance method of autonomous vehicle based on improved artificial potential field. Int J Adv Robot Syst. 2022;19(4):17298806221115984.
- 21. Tan G, Zhuang J, Zou J, Wan L, Sun Z. Artificial potential field-based swarm finding of the unmanned surface vehicles in the dynamic ocean environment. Int J Adv Robot Syst. 2020;17(3):1729881420925309
- 22. Azzabi A, Nouri K. An advanced potential field method proposed for mobile robot path planning. Trans Inst Measur Control. 2019;41(11):3132–44.
- 23. Zhao M, Zheng Z, Yao Q. Path planning method for multiple mobile robots based on modified potential field method. Appl Res Comput. 2020;37:66–72.
- 24.
Wang D, Chen H, Wu C. A path planning and obstacle avoidance method for USV based on dynamic-target APF algorithm in edge. International Conference on Algorithms and Architectures for Parallel Processing. Singapore: Springer; 2023. p. 21–39.
- 25. Sang H, You Y, Sun X. The hybrid path planning algorithm based on improved A* and artificial potential field for unmanned surface vehicle formations. Ocean Eng. 2021;223:108709.
- 26. Zhai L, Liu C, Zhang X. Local trajectory planning for obstacle avoidance of unmanned tracked vehicles based on artificial potential field method. IEEE Access. 2024;12:19665–81.
- 27. Wang J, Wang R, Lu D, Zhou H, Tao T. USV dynamic accurate obstacle avoidance based on improved velocity obstacle method. Electronics. 2022;11(17):2720.
- 28. Zhu H, Ding Y. Optimized dynamic collision avoidance algorithm for USV path planning. Sensors (Basel). 2023;23(9):4567. pmid:37177771
- 29. Wenming W, Jialu D, Yihan T. A dynamic collision avoidance solution scheme of unmanned surface vessels based on proactive velocity obstacle and set-based guidance. Ocean Eng. 2022;248:110794.
- 30. Xia G, Han Z, Zhao B, Wang X. Unmanned surface vehicle collision avoidance trajectory planning in an uncertain environment. IEEE Access. 2020;8:207844–57.
- 31. Li Y, Jin R, Xu X, Qian Y, Wang H, Xu S, et al. A mobile robot path planning algorithm based on improved A* algorithm and dynamic window approach. IEEE Access. 2022;10:57736–47.
- 32. Pei M, An H, Liu B, Wang C. An improved dyna-Q algorithm for mobile robot path planning in unknown dynamic environment. IEEE Trans Syst Man Cybern, Syst. 2022;52(7):4415–25.
- 33. Shi K, Wu Z, Jiang B, Karimi HR. Dynamic path planning of mobile robot based on improved simulated annealing algorithm. J Franklin Inst. 2023;360(6):4378–98.
- 34. Han D, Nie H, Chen J, Chen M. Dynamic obstacle avoidance for manipulators using distance calculation and discrete detection. Robot Comput-Integrat Manuf. 2018;49:98–104.
- 35. Chen P, Pei J, Lu W, Li M. A deep reinforcement learning based method for real-time path planning and dynamic obstacle avoidance. Neurocomputing. 2022;497:64–75.
- 36. Li S, Han K, Li X, Zhang S, Xiong Y, Xie Z. Hybrid trajectory replanning-based dynamic obstacle avoidance for physical human-robot interaction. J Intell Robot Syst. 2021;103(3):1–14.
- 37. Chen G, Liu D, Wang Y, Jia Q, Zhang X. Path planning method with obstacle avoidance for manipulators in dynamic environment. Int J Adv Robot Syst. 2018;15(6):1729881418820223.
- 38. Conant GC, Wolfe KH. Turning a hobby into a job: how duplicated genes find new functions. Nat Rev Genet. 2008;9(12):938–50. pmid:19015656
- 39. Zohaib M, Pasha SM, Javaid N. IBA: Intelligent bug algorithm–a novel strategy to navigate mobile robots autonomously. In: Communication Technologies, Information Security and Sustainable Development: Third International Multi-topic Conference, IMTIC 2013. 2014. p. 291–9.
- 40. Zohaib M, Ahsan M, Khan M, Iqbal J. A featureless approach for object detection and tracking in dynamic environments. PLoS One. 2023;18(1):e0280476. pmid:36649310
- 41. Zhai D-H, Xia Z, Wu H, Xia Y. A motion planning method for robots based on DMPs and modified obstacle-avoiding algorithm. IEEE Trans Automat Sci Eng. 2023;20(4):2678–88.