Local Path Planning: Dynamic Window Approach With Virtual Manipulators Considering Dynamic Obstacles

Local path planning considering static and dynamic obstacles for a mobile robot is one of challenging research topics. Conventional local path planning methods generate path candidates by assuming constant velocities for a certain period time. Therefore, path candidates consist of straight line and arc paths. These path candidates are not suitable for dynamic environments and narrow spaces. This paper proposes a novel local path planning method based on dynamic window approach with virtual manipulators (DWV). DWV consists of dynamic window approach (DWA) and virtual manipulator (VM). DWA is the local path planning method that performs obstacle avoidance for static obstacles under robot constraints. DWA also generates straight line and arc path candidates by assuming constant velocities. VM generates velocities of reflective motion by using virtual manipulators and environmental information. DWV generates path candidates by variable velocities modified by VM and predicted positions of static and dynamic obstacles. Therefore, in an environment with dynamic obstacles, the obstacle-avoidable paths which include non-straight line and non-arc paths are generated. The effectiveness of the proposed method was confirmed from simulation and experimental results.


I. INTRODUCTION
In recent years, research on service robots have been active due to infectious diseases and aging population. Service robots will be used more and more in the future to create a labor force instead of people [1]- [3]. These service robots are required to move around in dynamic environments and narrow spaces where robots can coexist with people [4].
Autonomous mobile robot systems are generally composed of localization [5], mapping [6], perception [7] and path planning [8]. Path planning has to consider static and dynamic obstacles. Path planning is roughly classified into two types; global path planning and local path planning. Global path planning generates a path that does not collide with obstacles based on a prior map [9]- [11]. However, global path planning cannot consider obstacles that do not The associate editor coordinating the review of this manuscript and approving it for publication was Liang Hu . exist in the prior map. Therefore, local path planning generates paths from sensors information in real-time. Local path planning deals with obstacles that are not on the prior map [12], [13].
This paper focuses on local path planning methods. Many local path planning methods were reported for the collision avoidance against static and dynamic obstacles [14], [15]. Fiorini and Shiller proposed a local path planning method for multi-robots called Velocity Obstacle (VO) [16]. VO considers dynamic obstacle avoidance by using velocity spaces. Berg et al. expanded VO for smooth and vibration-free path planning [17]. Xu et al. reported VO considering obstacles velocities that are larger than the maximum speed of the robot [18]. There are useful local path planning methods based on VO for multi-robot systems [16]- [19]. However, when VO methods generate velocity of obstacle-avoidable path candidates by using velocity space, robots velocities are assumed as constant. Thus, the path candidates of robots are a straight line and arc paths under the non-holonomic constraint. Therefore, the path candidates of robots decrease in narrow and dynamic environments because the robot collides with obstacles. These VO methods also do not consider dynamics constraints.
There are local path planning methods considering kinematics and dynamics constraints. Fox et al. reported dynamic window approach (DWA) [20]. DWA generates path candidates by using the velocity space with dynamics constraints (VSD). VSD is the velocity space that the robot can generate from current velocities. DWA selects the optimal path from path candidates. Dobrevski et al. reported local path planning based on DWA and deep reinforcement learning to improve path optimization [21]. Liu et al. developed the global dynamic path planning fusion algorithm combining jump-A* algorithm and DWA [22]. In addition, several useful local path planning methods based on DWA were reported [20]- [22]. However, DWA generates path candidates by assuming constant velocities for a certain period time. Therefore, path candidates are straight line and arc paths. These path candidates are not suitable in dynamic environments and narrow spaces, since these path candidates do not consider obstacles. In other words, many path candidates with collisions are generated in dynamic environments and narrow spaces.
There are local path planning methods to generate the path candidates including non-straight and non-arc paths. Howard et al. proposed State Lattice Planner (SLP) [23], [24]. SLP generates the path candidates by using data sets of robot states and robot constraints. However, SLP may not generate path candidates if there are obstacles near the robot. We reported the local path planning method based on virtual manipulators (VM) and DWA [25]. VM was reported by Yamazaki and Inaba [26]. In narrow spaces with static obstacles, our method achieved better results compared with DWA. However, our method did not consider dynamic obstacles and guarantee path candidates at velocities within VSD.
In order to consider static and dynamic obstacles and guarantee path candidates at velocities within VSD, this paper proposes dynamic window approach with virtual manipulators (DWV). DWV is a novel generation method of path candidates including non-straight and non-arc paths. DWV generates path candidates by variable velocities modified by VM and predicted positions of static and dynamic obstacles. Therefore, in an environment with static and dynamic obstacles, the obstacle-avoidable paths which include non-straight line and non-arc paths are generated. Extensive simulations and experiments were conducted to verify the effectiveness of DWV.
This paper consists of seven sections including this one. Section II shows the modeling of the robot. Section III explains DWA as the conventional method. Section IV proposes DWV. In Sections V and VI, simulation and experimental results are shown to confirm the usefulness of the proposed method. Section VII concludes this paper.  Fig. 1 shows the coordinate system of the robot. This paper defines the local coordinate system LC and the global coordinate system GB . The value in the global coordinate system is expressed as the superscript GB . The variable of the local coordinate system does not have the superscript. The origin of the global coordinate system is set as an initial robot position. The origin of the local coordinate system is set as the center point of both wheels. As shown in Fig. 1, ( GB x, GB y) and the angle GB θ refer to the position and angle of the robot in the global coordinate system. The velocities of the global coordinate system GBẋ , GBẏ , and GBθ are derived as follows.

II. MODELING OF WHEELED MOBILE ROBOT
where v and ω are translational and angular velocities. GB θ and ( GB x, GB y) after t seconds are calculated as follows.

III. DYNAMIC WINDOW APPROACH (DWA)
A. OVERVIEW OF DWA DWA is one of the practical local path planning methods [20]. Fig. 2 shows the overview of DWA. Fig. 2(a) shows the flowchart of DWA. In this paper, DWA is implemented by 3 steps.
• Velocity Space with Dynamics Constraint (Step 1) As shopwn in Fig. 2(b), the velocity space with dynamics constraint (VSD) is generated from current robot velocities and robot specifications.
• Path Candidates (Step 2) As shown in Fig. 2(c), DWA generates path candidates by assuming constant velocities within VSD. The color of path candidates is pink.
• Optimal Path (Step 3) VOLUME 10, 2022 As shown in Fig. 2(d), DWA selects the optimal path from path candidates by the cost function. The color of the optimal path, collision paths, and no collision paths are red, green, and yellow. By using the velocity of the optimal path, the robot moves while avoiding obstacles.   All of the velocity space S all , which is determined from the robot specification, is described as follows.
where V min , V max , min and max are the minimum and maximum translational and angular velocities.

2) STEP 1-2: VELOCITIES SPACE OF DYNAMIC WINDOW S dw
The velocities space of dynamic window S dw , which is the velocities the robot can generate velocities until the next time step, is expressed as follows.
where v res and ω res are translational and angular velocity responses as the current velocities. A max and Π max are maximum translational and angular acceleration. T represents time step.

3) STEP 1-3: VELOCITY SPACE WITH DYNAMICS CONSTRAINT S vsd
As shown in Fig. 2(b), S vsd is as follows.
where v ini and ω ini are minimum translational and angular velocities in S vsd . v end and ω end are maximum translational and angular velocities in S vsd .
where P rob i represents the i-th path candidate of the robot. X rob i , Y rob i , and rob i represent position and attitude of the robot. The detail of Algorithm 1 is explained as follows.
DWA divides the velocity space S vsd into N tlv for the translational velocity axis. The translational velocity v dwa is selected as follows.
where ∆v means step translational velocity. g (1 ≤ g ≤ N tlv ) represents number for translational velocity.
DWA divides the velocity space S vsd into N agv for angular velocity axis. The angular velocity ω dwa is selected as follows.
where ∆ω means step angular velocity. h (1 ≤ h ≤ N agv ) represents number for angular velocity. The i-th path candidate P rob i is calculated from (v dwa , ω dwa ). In Algorithm 1 (line 12), onePath(v dwa , ω dwa ) is expressed as follows.
T is calculated from the predicted time T max and time step T . By repeating these processes N all times, path candidates of the robot O rob are generated.

D. OPTIMAL PATH (STEP 3)
Path candidates of the robot O rob are evaluated by the cost function. The cost function is derived as follows. where W ang , W vel and W obs are weight coefficients. c ang represents the cost of the direction to the goal. c vel means the cost of the current velocity. c obs is the distance from the robot and the obstacle. The optimal path is chosen from path candidates by DWA to maximize the cost function (21). Finally, velocities of the optimal path define optimal translational and angular velocities v opt and ω opt . Velocity commands of DWA are expressed as follows.
where v cmd and ω cmd are translational and angular velocity commands. By using velocity commands of DWA, the robot reaches the goal position during avoiding obstacles.

IV. VIRTUAL MANIPULATORS (VM) A. CONECEPT OF VIRTUAL MANIPULATORS
The reflective motion using VM was reported by Yamazaki and Inaba [26]. VM consists of two types of manipulators; the virtual leader manipulator and virtual assistant manipulators. The virtual leader manipulator is used for path-following. Virtual assistant manipulators are used for obstacle avoidance. Fig. 3 shows the concept of VM.
1) The robot follows the desired path using the virtual leader manipulator (Fig. 3(a)).
2) The robot detects obstacles from the distance sensor ( Fig. 3(b)). 3) Virtual assistant manipulators generate obstacle avoidance movement when the distance between the obstacle and roots of virtual assistant manipulators is less than the threshold value D max (Fig. 3(c)). 4) Virtual assistant manipulators are deleted when the distance between the obstacle and roots of virtual assistant manipulators is larger than the threshold value D max (Fig. 3(d)).

B. GENERATION OF VIRTUAL MANIPULATORS
The reflexible motion using virtual 2 link manipulator is calculated as follows.
whereq is the state vector. This state vector is written aṡ q = v vm , ω vm ,θ 01 ,θ 02 ,θ 11 ,θ 12 , · · · ,θ n1 ,θ n2 T . v vm and ω vm mean the translational and the angular velocities command of the robot with virtual manipulators.θ n1 andθ n2 are angle velocities of n-th virtual manipulators. J # and represent the pseudo-inverse Jacobian matrix and the weight coefficients of the null-space. ref nj and θ res nj represent the reference and response angle of virtual manipulators. j represents 1st or 2nd joint number (j = 1, 2). The detail of the virtual manipulators method is described in the references [26].

V. DYNAMIC WINDOW APPROACH WITH VIRTUAL MANIPULATORS (DWV) A. OVERVIEW OF DWV
As shown in Figs. 2 and 4(a), the framework of the flowchart for DWV is the same as DWA. Moreover, DWV has made two changes from DWA, such as path candidates (Step 2) and optimal path (Step 3).

• Velocity Space with Dynamics Constraint (Step 1)
As shown in Fig. 4(b), VSD is generated from robot velocities and specifications. VSD is expressed by (10).
• Path Candidates (Step 2) As shown in Fig. 4(c), DWV generates the path candidates by using DWA with VM. The color of path candidates is pink. By using VM and predicted obstacle position, DWV generates path candidates considering static and dynamic obstacles.
• Optimal Path (Step 3) As shown in Fig. 4(d), DWV selects the optimal path from path candidates. The color of the optimal path and no collision paths is red and yellow. DWV designed the cost function to consider dynamic obstacles. By using the velocity of the optimal path, the robot moves while avoiding obstacles. The detail of each step is expressed in subsections V.B to V.C.  1) Firstly, DWV selects velocities from VSD. DWA also uses these velocities. In DWA, the velocities of the robot are assumed as constant. Thus, the path candidates of DWA are arc paths (Fig. 5(a)). 2) DWV uses VM for each step f to generate obstacleavoidable paths. VM generates velocities of obstacle avoidance by using next position of obstacles ( Fig. 5(b)).
2) STEP 2-2B (ALGORITHM 2 LINES 7-9) The angular velocity ω dwa is selected from VSD. O obs = [P obs 1 · · · P obs s · · · P obs s max ] T between f -th positions of obstacles and roots of VM, angular velocity ω vm is calculated from (24). The f -th angular velocity ω vm f are calculated as follows.
where v dwv and ω dwv f mean translational and the angular velocities of DWV. where The predicted robot position is calculated from v dwv and ω dwv f . The i-th path candidate P i is calculated as follows.
By using v dwv and ω dwv f , path candidates by DWV include non-straight line and non-arc paths as shown in Fig. 4(c), since DWV modifies angular velocities ω dwv f for each time step.

7) STEP 2-7B (ALGORITHM 2 LINE 29-35)
The collision between the robot and obstacles is checked. If the robot collides with obstacles, this path candidate is not used for the optimal path. By repeating these processes N all times, path candidates of the robot O rob are generated.

C. OPTIMAL PATH (STEP 3)
Path candidates of the robot O rob are evaluated by the cost function. DWV has adopted the distance to the goal position as a cost function to reduce the restrictions on avoiding robot obstacles. The cost function is derived as follows.
where W pos and W sdo are the weight coefficients of position and obstacles. c pos is the distance from the goal position and the predicted position of the robot at f max . c sdo is the shortest distance calculated from the predicted positions of the robot and obstacles. c sdo is calculated for each step f . DWV considers static and dynamic obstacles in the phase of  selecting an optimal path. The optimal path is selected from path candidates by DWV to maximize the cost function. Finally, velocities of the optimal path define optimal translational and angular velocity v opt and ω opt f . Velocity commands of DWV are expressed as follows.
where v opt and ω opt 1 are optimal velocities of 1st time step. By using velocity commands of DWV, the robot reaches the goal position during avoiding obstacles.  Table 1 shows control parameters. Robot Operating System (ROS) was used [27]- [30]. Fig. 6 shows the    path candidates and optimal path of SLP. Parameters of SLP path candidates are shown as follows. N pos , N hea and L pat represent the number of samples in terminal state position and heading, and the terminal position horizon. ϒ min , ϒ max , min and max represent the angular range of the terminal position sampling and the angular range of the terminal heading angle offsets. The detail of the SLP is described in the references [23], [24]. In this simulation, DWV was equipped with two VM. The root position of 1st and 2nd VM in robot coordinate system were (x vam 1 , y vam 1 ) = (0.0, −0.1) and (x vam 2 , y vam 2 ) = (0.0, 0.1).

B. SIMULATION SETUP
As shown in Table 2, there are four simulation cases. Fig. 7 shows the simulation environment in Cases S1-S4. In Fig. 7, the gray and blue circles are the robot and VOLUME 10, 2022  , it is judged as a goal. The simulation cases are defined as follows.
• Case S1: Static Obstacles As shown in Fig. 7(a), there are 10 static obstacles in the simulation environment.
• Case S2: Dynamic Obstacles As shown in Fig. 7 Case S1 and Case S2 were simulated 1 times. Case S3 and Case S4 were simulated 100 times.  Table 3 shows simulation results of all cases. Table 3 contains the success rate in reaching the goal without the collision, the average travel time only in reaching the goal, the trajectory length (TL) only in reaching the goal, and the movement posture displacement (PD) only in reaching the goal.

Figs
In Case S1, all methods reached the goal position as shown in Figs. 8-10(a). SLP reached the goal position earlier than DWA and DWV. This is because the SLP moved at maximum translational velocity on the optimal path. From Table 3 and Fig. 8(a), the best result in Case S1 was obtained by SLP.
In Case S2, DWV reached the goal position as shown in Figs. 8-10(b). From Figs. 8-9(b), SLP and DWA did not reach  the goal position. SLP and DWA generated path candidates not considering dynamic obstacles. SLP and DWA considered dynamic obstacles when the optimal path was selected from path candidates. However, there were no path candidates without collision. Therefore, SLP and DWA collided with dynamic obstacles. On the other hand, DWV generated path candidates considering dynamic obstacles. DWV also considered dynamic obstacles when the optimal path was selected from path candidates. Thus, DWV reached the goal position. From Table 3 and Fig. 10(b), the best results in Case S2 was obtained by DWV.
In Case S3, all method reached the goal position as shown in Figs. 8-10(c). As shown in Fig. 8(c), the goal trajectories of SLP were close to straight lines. The position and velocity of obstacles were given randomly for 100 simulation times. There were a few easy situations for SLP, such as obstacles did not move towards the robot. Fig. 9(c) shows the goal trajectories of DWA. When DWA reached the goal position with avoiding moving obstacles, DWA sometimes generated back movements. The goal time of DWA was longer than other methods. From Table 3 and Fig. 10(c), DWV had the highest success rate of reaching the goal at 85%. The best result in Case S3 was obtained by DWV.
In Case S4, all method reached the goal position as shown in Figs. 8-10(d). As shown in Figs. 8-9(d), the goal trajectories of SLP and DWA were close to straight lines. The position and velocity of obstacles were given randomly for 100 simulation times. There were a few easy situations for SLP and DWA, such as obstacles did not move towards the robot. Moreover, DWV sometimes collided with obstacles in Case S4. However, DWV had the highest success rate of reaching the goal at 70% as shown in Table 3 and Fig. 10(d). The best result in Case S4 was obtained by DWV.
The effectiveness of the proposed method was confirmed from the simulation results.

A. EXPERIMENT SYSTEM
As shown in Fig. 11(a), Turtlebot2 [31] and ROS Melodic were used in these experiments. Turtlebot2 was equipped with the laser range finder (LRF) that was URG-04LX-UG01 [32]. The Range of LRF was shown in Fig. 11(b). Fig. 11(c) shows the system configuration. Firstly, environmental information was obtained by LRF. Secondly, the obstacle position and velocities were generated by obstacle_detector [33]. In these experiments, obasta-cle_detector got only static and dynamic obstacles that radius was less than 0.25 [m]. Finally, DWV generated the velocity command considering static and dynamic obstacles from obastacle_detector and LRF information. As shown in Table 1, the same parameters as in the simulation were set for these experiments.

B. EXPERIMENT SETUP
There were two experiment cases. The start and goal position was ( GB x start , GB y start ) = (0.0, 0.0) and ( GB x goal , GB y goal ) = In Case E1, there were 2 static obstacles, 1 pedestrian, and walls in the narrow space. As shown in Figs. 12(c)-(g), the robot reached the goal without collision. From Fig. 12(d), DWV generated path candidates considering Obstacle 1 and walls. From Fig. 12(e), the robot avoided Obstacles 1. DWV generated path candidates considering P-1, Obstacle 2, and the walls. From Fig. 12(f), DWV generated the path candidates avoiding P-1, Obstacle 2, and the walls. From Fig. 12(g), the robot avoided P-1. DWV generated path candidates considering Obstacle 2. Finally, the robot reached the goal position without collision.
In Case E2, there were 4 pedestrians and walls in the narrow space. As shown in Figs. 13(c)-(g), the robot reached the goal without collision. From Fig. 13(d), DWV generated path candidates considering P-2A, so the robot moved backward. From Fig. 13(e), the robot avoided P-2A. DWV generated path candidates considering P-2A∼ 2D and walls. From Fig. 13(f), the robot avoided P-2A and P-2C. DWV generated the path candidates considering P-2A, P-2B, and P-2D. From Fig. 13(g), the robot avoided P-2A, P-2B, and P-2D. DWV generated path candidates considering P-2B and walls. Finally, the robot reached the goal position without collision.
The effectiveness of the proposed method was confirmed from the experiment results.

VIII. CONCLUSION
This paper proposed the novel local path planning method called DWV. DWV was composed of DWA and VM for static and dynamic obstacle avoidance. DWV generated obstacle-avoidable path candidates which include non-straight line and non-arc paths. These path candidates were generated by variable velocities considering predicted positions of static and dynamic obstacles. DWV considered kinematics and dynamics constraints. The effectiveness of the proposed method was demonstrated by simulations and experiments.
In future works, we will work to evaluate DWV as follows.
• Parameter Design of DWV The number of parameters was increased by considering virtual manipulators. The parameter design method should be clarified.

• DWV with Global Path Planning
We evaluated DWV using only the local path planning method. We will apply both DWV and global path planning.

• Various Environments, Robots and Sensors
We evaluated DWV with the robot and environments. We will evaluate DWV for various robots, environments, and sensors.