An improved dynamic window approach for local trajectory planning in the environment with dense objects

The Dynamic Window Approach (DWA) has been one of the most popular solutions in local trajectory planning due to the advantages of movement fluency. However, the traditional DWA faces the challenge of low-efficiency in the case of local trajectory planning since the robot cannot perceive the density of the obstacle. In this paper, we propose an improved DWA to solve this problem. First, we use multi-sensor technology and new evaluation algorithms to enable the capability of density perception for the disorderly environment. Second, the evaluation factor related to the density change is introduced into the path evaluation function, which will facilitate the robot to perceive the distribution of dense objects in advance. Computer simulation and field experiments show that the efficiency of the improved DWA is increased by 25%. The improved DWA not only considers the original factors such as heading angle, but also introduces a density factor to evaluate the next path, so as to avoid entering dense areas in advance. It can be seen that the improved DWA has relatively stable speed and acceleration, and can avoid dense areas in advance, which can be widely used in robots running in dense environments.


Introduction
Mobile robots are widely used in many fields, especially in hospitals for transportation [1,2]. However, with the overloaded operation of hospitals, mobile robots cannot plan and move in crowded places. Due to the movement of dense crowds, the walking of mobile robots is severely restricted. Path planning is one of the important components of autonomous navigation for mobile robots, which determines whether the robot can avoid obstacles in an unknown environment [3]. According to the degree of grasp of information, path planning can be divided into global trajectory planning based on a priori complete information and local trajectory planning based on sensor information [4]. In an unstructured environment, mobile robots perceive obstacles based on sensors and plan local paths.
Currently, local trajectory planning includes artificial potential field method [5], dynamic window approach [6][7][8], and time elastic band method [9]. Traditional local trajectory planning algorithms only complete basic navigation tasks without considering complex application scenarios.
To avoid static obstacles and differently arranged walls, Cong Y Z et al. developed an ant colony optimization (ACO) metaheuristic method [10]. Tamura, Y. et al. Proposed an improved PFS 2. Machine architecture Before introducing our improved DWA, we provide the details of our in-house developed mobile robot for our proposed algorithm. To maximize computing power, minimize cost, and most conveniently update algorithms later, mobile robots need to allocate sensor inputs reasonably. In order to improve the adaptability and development efficiency of the algorithm, this article refers to the general machine architecture. The components of the robot are shown in Figure 1.  The robot adopts a modular design concept and consists of three parallel control units. The top module is equipped with a PC or mobile terminal, responsible for robot scheduling and humancomputer interaction, and communicates with the middle-layer module through WIFI. The middlelayer module is based on Jetson Nano, which collects 3D camera and lidar information, to realize the navigation algorithm of SLAM and plan planning. The bottom-layer module is based on the STM32 development board, which samples the information from the Inertial Measurement Unit (IMU) to realize the robot's self-positioning and to control the motors in real-time. The avoidances of dense objects require the robot to obtain information around the robot in real time. The global path planning algorithm navigates based on the original information map. The local trajectory planning algorithm navigates based on real-time sensor information and global planning points. Therefore, it is necessary to study a suitable multi-sensor fusion strategy for dense objects. The multi-sensor fusion strategy adopted by the robot in this article first fused the information of the three sensors, then through the back-end optimization, loop detection, and finally the map information. The multi-sensor fusion process in this article is shown in Figure 2.

DWA
The DWA concept is to directly search for the optimal control command that maximizes the objective function in the command area. The principle of the dynamic window approach is to collect multiple speed samples in the vector space of the mobile robot's motion speed, simulate the possible driving paths of the mobile robot over a while, and evaluate these possible paths to select the optimal trajectory speed samples. Then control the movement of the mobile robot.
The speed control command of the robot is generated by searching in the speed space. The speed space is constrained by three conditions, including maximum and minimum speed constraints, dynamic performance constraints, and safety constraints. The maximum and minimum speed constraints are derived from the safety values preset by the program. As shown in formula (1). The reason for the dynamic performance constraint is the limitation of motor performance. As shown in formula (2). The reason for the safety constraint is that the braking distance between the robot and the obstacle needs to be maintained. As shown in formula (3). (1) ( 3 ) Where v and w are the linear velocity and angular velocity of the robot. Among them, dist(v,w) represents the distance between the trajectory corresponding to the speed pair and the nearest obstacle. The safety constraint first samples to obtain multiple sets of speeds, and then calculates the value of dist(v,w). If the trajectory collides with obstacles, the speed pair is discarded. When DWA calculates the corresponding trajectory of the speed pair, it is assumed that the value of the speed pair does not change during the operation period, and the speed pair is not updated until the next operation period. Combining the above three constraints, the velocity space pair can be obtained by running the intersection calculation. As shown in formula (4).
(4) Establishing trajectory evaluation mathematical function: the trajectory evaluation function calculates the heading, distance, and speed according to each group of speed pairs in the speed sampling space, judges and selects an optimal trajectory, and then drives the robot to drive at the corresponding speed pair. As shown in formula (5).
( 5 ) Among them, heading(v, w) represents the angle between the end of the trajectory corresponding to the speed pair (v, w) and the target position. Its function is to select a trajectory with a smaller angle to the target position and make the robot move towards Drive to the target location. dist(v, w) represents the distance between the robot and the obstacle, and its function is to keep the robot away from the obstacle. velocity(v, w) represents the value of velocity to (v, w) centreline velocity, the purpose is to make the robot travel to the target position at a faster speed. Among them,α, β, and γare the weight coefficients of heading(v, w), dist(v, w), and velocity(v, w) respectively. The three weight coefficients need to be normalized. The trajectory evaluation function takes the trajectory with the highest score in the weighted summation of the three factors. The speed pair (v, w) at the next moment is published to the ROS node to complete the local trajectory planning.

Problems with the DWA
When the two-wheeled differential robot uses DWA navigation, we found that the following difficulties will arise when planning the path: once dense objects appear on the robot's global planning path, the robot is easily trapped. Since the robot cannot judge the density of the environment, the robot needs more turning, acceleration, and deceleration operations to get out of trouble. As shown by the blue dashed line in Figure 3.
Since the robot cannot perceive the increase in the density of objects, it will not plan and bypass the dense area in advance, but enter the dense area. The original DWA path is shown in the blue trajectory in Figure 3. When DWA encounters obstacles, it only considers factors such as heading angle, target distance and speed, which leads to entering a dense area. Choosing dense and tortuous obstacle paths will consume the motor unnecessarily. Although the robot can pass safely, this partial path is not the best obstacle avoidance path. This is because the path of the global path planning does not consider the robot volume and the density of objects, which will seriously affect the optimal solution range of the local trajectory planning. More importantly, the robot only considers the heading angle, obstacle distance, and linear velocity. The native DWA does not consider the density of objects in the environment, which will inevitably lead to the robot being easily trapped or extremely time-consuming.

Improvement of DWA
To the simplicity and classic of the DWA principle, it has been widely used in practical situations. However, based on the analysis of the above drawbacks, we found that DWA still has a lot of room for improvement. We add the density evaluation factor to the evaluation function. We can first judge whether the k-th obstacle is in the dense area according to the coordinate position between the obstacles. The calculation is as follows: (6) Where, , represents the coordinate information of the k-th obstacle in the dense area and represents the expansion distance of the obstacle. The unit of heading(v,w) is radians or angles, the unit of dist(v,w) is m, the unit of velocity(v, w) is m/s, and the unit of density(v,w) is /m. Therefore, the meanings and units of the four evaluation factors are different. We refer to the original DWA and normalize the four evaluation factors to ensure consistent standards and eliminate coupling.
The increase in density allows the robot to incorporate the density of obstacles into the DWA when choosing a path. The trajectory evaluation function of the improved DWA is as follows: The text of your paper should be formatted as follows: ,

Simulation experiment
The above analysis shows that the improved algorithm is feasible. Therefore, we conducted a simulation experiment of algorithm performance in MATLAB version 2015b. The computer configuration is Intel Core i7-5500U 2.40GHz, 16GB.  Data refresh frequency 10Hz From Figure 3, DWA and improved DWA correspond to two different path trajectories. Obviously, the improved DWA can make the robot avoid dense objects, reduce the turning range, reduce acceleration and deceleration operations, thereby reducing the total time and increasing efficiency. This is because the trajectory evaluation function introduces a density factor. The density factor will feed back the density of obstacles in the current trajectory to the robot. The robot combines heading angle, target distance, speed factor and density factor to select the best trajectory. The improved DWA reduces the amplitude of the direction change and maintains one direction as a whole, thus achieving the smooth operation of the robot. Comparing the two trajectories in Figure 4, the improved DWA has a smaller heading angle fluctuation, and the slope of is smoother. The value of remains above zero and there is no positive or negative change, so the motor always keeps forward or reverse rotation. Comparing the two trajectories in Figure 5, the improved DWA improves the average speed of the robot and improves the operating efficiency. By changing different target points, we further verified the advantages of the improved DWA. The results are shown in Table 2.

Field experiment
To evaluate the robot's navigation and obstacle avoidance performance in a dense environment, and compare the performance difference between DWA and improved DWA, we abstracted and simulated the real environment, and established an environment with dense objects in the laboratory. Experiments show that DWA and improved DWA have different navigation planning strategies. The improved DWA can avoid dense objects and prevent trapping into dense environments. Figure 6 is an experimental photo using DWA navigation. Figure 7 is an experimental photo using the improved DWA navigation.  Figure 6. Experimental photo using DWA navigation. Figure 7. Experimental photo using the improved DWA navigation. The navigation process of DWA is shown in Figure 6. The experimental results of DWA are consistent with the simulation results. The robot chooses the next path according to the original DWA evaluation formula. When encountering obstacles, the robot prioritizes the three factors of heading angle, target distance and speed, which causes the robot to enter a dense area. The robot can still pass through this dense area, but requires multiple acceleration, deceleration and turning operations, which causes the robot to travel unevenly and time-consuming. The experimental results of the improved DWA are consistent with the simulation results. The improved DWA improves the shortcomings of the original DWA, and increases the density factor to make the robot avoid dense areas. According to the improved DWA evaluation formula, through the feedback of sensor information, the robot can perceive dense areas in advance and adjust the local trajectory planning in time. Since the robot does not enter the dense area, the robot maintains a relatively stable speed and acceleration. Compared with the improved DWA and DWA, the time and mileage of the improved DWA are smaller. Experiments show that the new method proposed in this paper can effectively enhances the ability to avoid dense areas in advance.

Conclusions
Aiming at the environment of dense objects, this paper improves the DWA trajectory evaluation function, the purpose is to solve the problem that the local trajectory planning algorithm DWA cannot avoid dense objects in advance. This paper proposes a density factor to measure the density of the environment. The density factor predicts the dense area based on the update of sensor information, pre-perceives the distribution of dense objects, reduces the possibility of the robot falling into the dense area, and improves the overall operating efficiency of the robot. In the existing trajectory evaluation function, this paper introduces a density factor and normalizes it. From the robot simulation experiment and field experiment, the improved DWA overcomes the shortcomings of the DWA and can be widely used in robots running in dense environments.