Computationally Efficient Dynamic Window Approach Based on Pattern Search Optimization

Autonomous ground vehicles (AGVs) are rapidly advancing to provide efficient and safe operations in various industrial sectors, including logistics, manufacturing, and healthcare [1–2]. The navigation of an AGV can be divided into two parts: local and global path planning [3]. The global one is responsible for providing the shortest path to the goal based on the environment map [4]. The AGV then follows the global path using the sensors onboard to avoid unpredictable obstacles – this is known as a local path planning problem [5–6].

In this paper, the local path planning problem is considered. One of the most commonly used algorithms is called the Dynamic Window Approach (DWA) [7]. It is worth noting that DWA is one of the default local path planning algorithms implemented in the Robot Operating System. The algorithm is based on selection control signals (i.e., linear and angular velocity pairs) to minimize the objective function. The procedure is as follows: Generate possible pairs of velocities with particular emphasis on limited dynamics (i.e., linear and angular accelerations). Next, for each pair, predict the future trajectory and calculate the objective function. The objectives are as follows: to maximize the linear velocities, maximize the clearance, and minimize the heading angle of movement to the goal. The algorithm is relatively simple. However, to provide high performance, the number of pairs for evaluation must be relatively high. In such a case, the computational effort is significant.

In recent years, researchers have focused on enhancing the DWA algorithm to achieve the highest performance in AGV operations. However, there is a gap related to the computational performance of the DWA. In [8], the integration of reinforcement learning with an improved dynamic window approach is proposed to plan the path of mobile robots in unknown environments. Using learning policies (Q-learning algorithm), the AGV can adjust the weight of the objective function of the DWA. This solution enables the authors to enhance the efficiency and effectiveness of robotic navigation in complex and unpredictable terrains. Unfortunately, such a solution increases the computational effort of the algorithm. An improved DWA for unmanned surface vehicles (USVs) is proposed by incorporating environmental factors such as wind, waves, and current into the local path planning process [9]. Additional factors were added to the objective function of the DWA. This modification enables USVs to more effectively adapt their navigation strategies in real-time, thereby significantly enhancing their ability to maneuver through complex and dynamic maritime environments. The computational effort was not considered in the article. In [10], the smooth and safe path is provided using a path planner based on DWA and A* algorithms. The solution by the fusion algorithm can successfully avoid dynamic obstacles and is effective and feasible in path planning. A similar approach was proposed in [11]. However, the proposed mechanism appears more complex, and enhancements significantly improve global planning efficiency, ensuring smoother paths. In [12], the DWA was combined with the rapidly-exploring random tree (RRT*) algorithm. The modification was similar to the ones mentioned above. The RRT* was used to provide a global path, while the DWA algorithm was responsible for tracking the global path. Moreover, the object recognition using the YOLO framework was adopted. The presented results demonstrate that the DWA algorithm is a crucial path-planning algorithm for a broad range of autonomous vehicles. The objective of the work described in [13 is to design an improved DWA algorithm to enhance the obstacle avoidance ability of mobile robot formations when encountering dynamic obstacle interference. In [14], enhanced DWA is proposed to improve performance in environments filled with dense objects. An additional component of the DWA objective function related to object density is introduced. Improvements enable more effective real-time navigation in complex environments, resulting in increased safety and operational efficiency for AGVs. The calculation time of the algorithm was not taken into account. The algorithm proposed in [15] integrates the Particle Swarm Optimization (PSO) algorithm with an improved DWA to achieve effective planning of AGV routes in a dynamic environment. The PSO has been used for global path planning, while the DWA has been used to track the optimal trajectory. Improvement in the DWA was related to modifying the objective function of the DWA to account for dynamic obstacle velocities. The computational effort has not been presented in the paper. Dynamic adaptive DWA was proposed in [16]. The algorithm enhances the DWA by introducing adaptive mechanisms that dynamically adjust its prediction horizon in response to environmental conditions and obstacle information. It allows the authors to improve the DWA algorithm in densely packed environments. Furthermore, the article proposes applying fuzzy logic to adapt the weights of the DWA objective function. Unfortunately, the calculation time of the modification was not presented in comparison to the original DWA.

The original DWA method for selecting the optimal pair of velocities is not computationally efficient. To improve the selection process, the author proposes an approach based on the Pattern Search (PS) optimization algorithm. The algorithm does not require a gradient calculation, and it can efficiently solve the local optimization problem. The algorithm calculates the candidates with a predefined step size in each dimension. If the algorithm finds a better solution, the current position is switched to the new one. Otherwise, the step size is reduced (divided by 2) and the procedure is repeated until the predefined resolution or the maximum number of function evaluations is reached. In this paper, the pattern search dynamic window approach (PSDWA) is proposed. The proposed approach enables a significant reduction in the computational effort required by the algorithm.

The paper is organized as follows. Section 2 describes the original Dynamic Window Approach. The proposed PSDWA is presented in Section 3, while the simulation results, along with a comparison to the original DWA, are presented in Section 4. The paper is summarized in Section 5.

The Dynamic Window Approach is a fundamental local path planning algorithm and is included as one of the default libraries in the Robot Operating System (ROS). DWA incorporates the prediction of future movements based on the specified linear and angular velocities of the AGV. The algorithm itself can be divided into two primary components:

• –

  Search within the solution space,

• –

  Optimization.

The first component is responsible for preparing a discrete set of possible control signals, i.e., linear and angular velocities. It comprises the following three elements:

• –

  Circular trajectories: A key assumption of the dynamic window approach is the consideration of circular trajectories, which can be defined as pairs of linear and angular velocities (ν, ω). This assumption reduces the path planning problem to a two-dimensional one.

• –

  Admissible velocities: A given velocity pair is considered only if the resulting trajectory is deemed safe, which means that it does not result in a collision with any obstacle. The admissibility condition for a velocity pair (ν, ω) is that the AGV must be able to stop before reaching the closest obstacle on the generated trajectory.

• –

  Dynamic window: Limited AGV accelerations are assumed. Therefore, considering the AGV’s current velocities, future velocities are allowed to change only within a bounded range defined by the maximum linear and angular accelerations.

The second component, namely optimisation, involves maximizing a cost function expressed by the following equation: 1G(v,ω)=σ(α· heading (v,ω)+β·dist(v,ω)+γ·vel(v,ω))G(v,\omega) = \sigma (\alpha \;\cdot{\rm{ heading }}(v,\omega) + \beta \;\cdot\;{\mathop{\rm dist}\nolimits} (v,\omega) + \gamma \;\cdot\;{\mathop{\rm vel}\nolimits} (v,\omega)) where: α, β, and γ are weighting parameters of the cost function, and σ denotes a smoothing function to keep the subobjectives (i.e. heading, dist and vel functions) in range < 0,1 > for entire DWA iteration. This cost function comprises the following components:

• –

  Target heading: A reward is assigned for movement directed toward the target. The motion toward the goal gives the maximum value.

• –

  Clearance: The dist function represents the minimum distance to an obstacle along a given trajectory. The smaller the distance to the obstacle, the higher the likelihood that the AGV will attempt to maneuver around it.

• –

  Velocity: A reward is provided for a higher linear velocity in the cost function.

First, the algorithm generates possible linear and angular velocities within the maximum capabilities of the AGV. Additionally, assuming that the robot is currently moving with velocities (ν_AGV, ω_AGV) and that the maximum linear and angular accelerations are (α^max, ε^max), the upper and lower bounds for admissible velocities can be defined as: 2vAGVmax(k+1)=vAGV(k)+amax·Tsv_{AGV}^{max}(k + 1) = {v_{AGV}}(k) + {a^{max}}\;\cdot\;{T_s} 3vAGVmin(k+1)=vAGV(k)−amax·Tsv_{AGV}^{min}(k + 1) = {v_{AGV}}(k) - {a^{max}}\;\cdot\;{T_s} 4ωAGVmax(k+1)=ωAGV(k)+ϵmax·Ts\omega _{AGV}^{max}(k + 1) = {\omega _{AGV}}(k) + {^{max}}\cdot{T_s} 5ωAGVmin(k+1)=ωAGV(k)−ϵmax·Ts\omega _{AGV}^{min}(k + 1) = {\omega _{AGV}}(k) - {^{max\;}}\cdot\;{T_s} where: T_s denotes the time at the k-th sample time. In this manner, the dynamic window defining the admissible range of linear and angular velocities is obtained. When this dynamic window is intersected with the maximum allowed AGV velocities, the set of permissible velocities (that is, the search space dictated by dynamics) is obtained. Therefore, two-dimensional search space is obtained by discretizing this range with a given resolution or dividing it into N_v and N_ω values for linear and angular velocities, respectively.

The next step involves predicting future positions (that is, the trajectory of the AGV) using the following equations: 6θ(k+1)=θ(k)+ωAGV(k)·Ts\theta (k + 1) = \theta (k) + {\omega _{AGV}}(k)\;\cdot\;{{\rm{T}}_{\rm{s}}} 7xAGV(k+1)=xAGV(k)+vAGV(k)·cos(θ(k+1))·Ts{x_{AGV}}(k + 1) = {x_{AGV}}(k) + {v_{AGV}}(k)\;\cdot\;cos(\theta (k + 1))\;\cdot\;{T_s} 8yAGV(k+1)=yAGV(k)+vAGV(k)·sin(θ(k+1))·Ts{y_{AGV}}(k + 1) = {y_{AGV}}(k) + {v_{AGV}}(k)\;\cdot\;sin(\theta (k + 1))\;\cdot\;{T_s} where: θ denotes the AGV’s orientation, and x_AGV and y_AGV are the robot's positions along the x and y axes respectively. Predictions are carried out over predefined time interval (H^prediction). If a trajectory has been generated for every pair (ν, ω), it can then be verified whether the trajectory is collision-free, and whether the AGV is capable of stopping before encountering an obstacle. The second condition can be expressed as the following condition: 9v≤2·dist(v,ω)·abv \le \sqrt {2\;\cdot\;{\mathop{\rm dist}\nolimits} (v,\omega)\;\cdot\;{a_b}} where: a_b denotes the deceleration values during braking. For a simplification the following assumption can be made: a_b = a^max. Next, by excluding collision-prone trajectories, a set of linear and angular velocity pairs is obtained for the optimization process. For each pair, a quality index value is computed using eq. (1), and subsequently, the pair with the maximum value among all evaluated options is selected. This determines the new linear and angular velocity of the robot. The procedure of the DWA algorithm is presented in Fig. 1 in graphical representation for a single linear velocity for better readability.

Fig. 1.

Visualisation of the DWA procedure for a single linear velocity

The most computationally complex part of the algorithm is to predict the future trajectory and calculate the objective function for each pair of possible linear and angular velocities. Commonly, the fixed number of division of linear and angular velocities (N_ν and N_ω, respectively) are used. For example, the default values for Robot Operating System are as follows: N_ν = 3 and N_ω = 10. Therefore, the 30 trajectories must be predicted to calculate the objective functions and select the best.

The proposed approach aims to reduce the computational effort of the DWA algorithm by integrating it with the Pattern Search optimization algorithm. The PS algorithm is employed to efficiently select new control signals for the AGV, rather than evaluating all possible solutions.

The Pattern Search algorithm is a direct method of finding the extremum of a function. It is a gradient-free optimization algorithm. The initial guess position and the initial step-size (Δ) are the only parameters of PS. Next, the neighborhood of the current point is examined. In the case of a better value in the neighborhood, the algorithm moves the current position to this point. When no improvement in the function's value is observed, the algorithm decreases the step size by a factor of two. The process is repeated iteratively until a stopping criterion is reached. PS is characterized by simplicity of implementation and robustness to the lack of smoothness of the objective function. However, its effectiveness may be limited for problems with high dimensionality or in the presence of local minima.

The most time-consuming part of the DWA algorithm is related to the optimization process. The original DWA algorithm determines the possible velocities and then calculates the objective function for each of these possibilities. To improve this part of the algorithm, the above-described PS optimization algorithm is applied. Therefore, the following possibilities are examined only if necessary. The procedure of the proposed PSDWA is as follows:

• –

  Calculate the possible linear and angular velocities using eq. (2)–(5).

• –

  Set the initial position for PS as the current linear and angular velocities.

• –

  Set the initial step size as a quarter of the linear and angular range.

• –

  Run PS, while the maximum number of evaluated solutions are not reached (iter^max).

• –

  Set the current position of the PS algorithm as linear and angular velocities.

It should be noted that the DWA’s objective function of DWA (eq. (1)) requires a smoothing function (σ). Such a function normalizes the values of the subobjectives to range < 0,1 > using the minimum and maximum value obtained in the examined solutions. In the proposed approach, the objective functions are calculated and compared sequentially. In such a case the smoothing function is not simple to determine, due to lack of information of the maximum and minimum value of examined objective functions. To address this issue, Author proposes to normalize the components using constant values: maximum allowed AGV’s linear velocity (ν^max) for velocity, π for heading angle, and obstacle distance reaction (dobstaclemin)\left({d_{obstacle}^{min}} \right) for clearance.

The effectiveness of the proposed PSDWA test and the comparison with the original DWA will be presented. The validation was executed in a MATLAB environment. The source code has been published on MathWorks FileExchange [18]. To ensure that the proposed approach allows AGV to reach similar performance, the examination has been provided in a thousand randomly generated environments with ten static obstacles. The algorithms’ parameters are presented in Tab. 1.

Tab. 1.

Parameter	Symbol	Value
Sampling period	T_s	0.01 s
Horizon of prediction	Hprediction	1.0 s
Maximum linear acceleration	amax	1.0 m/s2
Maximum linear velocity	vmax	0.5 m/s
Maximum angular acceleration	εmax	1.0 · π rad/s2
Maximum angular velocity	ωmax	0.5 · π rad/s
Obstacle distance reaction	dobstaclemind_{obstacle}^{min}	2.0
DWA: number of samples for linear velocity	Nv	3
DWA: number of samples for angular velocity	Nω	10
PSDWA: maximum number of examinations	itermax	15
PSDWA: Initial step size	ΔInitial	14[ vAGVmax−vAGVminωAGVmax−ωAGVmin ]{1 \over 4}\left[ {\matrix{ {v_{AGV}^{max} - v_{AGV}^{min}} \cr {\omega _{AGV}^{max} - \omega _{AGV}^{min}} \cr } } \right]

For comparison, statistical indicators commonly used for path planning algorithms have been used [15]:

• –

  path length: L=∑i=1M−1d(qix,y,qi+1x,y)L = \sum\nolimits_{i = 1}^{M\;\; - \;1} {{\rm{d}}\left({q_i^{x,y},q_{i + 1}^{x,y}} \right)} ,

• –

  smoothness: S=1M−2∑i=1M−1(qiθ−qi+1θ)2,S = \sqrt {{1 \over {M - 2}}\sum\nolimits_{i = 1}^{M\; - \;1} {{{\left({q_i^\theta - q_{i + 1}^\theta } \right)}^2}} } ,

qix,yq_i^{{\rm{x}},{\rm{y}}}

qiθq_i^\theta

The maximum number of examinations has been selected empirically in the above-described comparison. The specified value has been chosen to minimize computational effort while maintaining the high performance of the algorithm. However, the computational effort can be reduced if the lower performance is acceptable, or the higher performance can be achieved if the higher computational effort is possible.

In Fig. 2, examples of paths obtained by DWA and PSDWA are presented. The graphical representation of the AGV’s paths obtained for the original DWA and the proposed PSDWA confirms the performance of the PSDWA regardless of the reduced computational requirements.

Fig. 2.

Example trajectories of the AGV obtained for the original DWA and the proposed PSDWA

It is worth noting that the computational effort of the DWA is strongly dependent on the density of obstacles. Therefore, a performance sensitivity analysis under different obstacle distributions has been conducted, and the results are presented in Table 3. The higher number of obstacles in the environment significantly increases computational time due to the prediction and optimization processes. However, the proposed modification decreases the number of predicted paths. In such a case, the speed-up indicator appears to be constant, regardless of the number of obstacles. For the two obstacles, the speed-up indicator is slightly lower due to the other parts of the algorithm, such as the prediction path and optimization process.

The speed-up indicator is close to two due to the DWA number of velocity pairs being equal to 30, while for the proposed PSDWA, the maximum number of examinations was set to 15. This number can be increased or decreased. The higher number enables higher performance in local path planning, but it requires more computational resources. To illustrate its relationships, a comparison of path quality indicators and the maximum number of examinations is provided in Table 4.

In this paper, the combination of the Dynamic Window Approach and the Pattern Search optimization algorithm was proposed. To reduce the computational effort of the DWA algorithm, the procedure for selecting the next control signals (i.e., linear and angular velocities of the AGV) is based on a gradient-free optimization algorithm, specifically PS. The simulation examinations demonstrate that the proposed approach enables a reduction in calculation time by approximately two times, while maintaining the provided solution's smoothness as in the original DWA implementation. Moreover, the validation of the provided speed-up indicator for the proposed approach has been provided using environments with different obstacle densities. The maximum number of examinations has been examined to present possibilities for achieving a higher performance of the AGV with a similar calculation time to the original DWA.

It should be noted that the proposed approach can be implemented with more complex modifications of the DWA. The literature review in the Introduction section demonstrates that the improvement of the DWA algorithm primarily relies on modifications to the objective functions. Therefore, the selection based on the PS algorithm is still applicable to achieve a lower computational effort and higher AGV efficiency.

Future research will focus on experimental validation of the proposed approach using a real AGV and implementing PS with more complex DWA improvements proposed in the literature.