Fast Adaptable Mobile Robot Navigation in Dynamic Environment

07/19/2020 ∙ by Xihan Ma, et al. ∙ 0

Autonomous navigation in dynamic environment heavily depends on the environment and its topology. Prior knowledge of the environment is not usually accurate as the environment keeps evolving in time. Since robot is continuously evaluating the environment as it proceeds, deciding the optimal way to traverse the environment to get to the goal, computationally efficient yet mathematically adaptive navigation algorithms are needed. In this paper, a navigation scheme for mobile robot, capable of dealing with time variant environment is proposed. This approach consists of a global planner (A*) and local planner (VFH) to assure an optimal and collision-free robot motion. The algorithm is tested both in simulation and experimentation in different environments that are known to result in failures in VFH and ROS navigation stack, for comparison purposes. Overall, the algorithm enables the robot to get to the goal faster and also produces a smoother path while doing so.



There are no comments yet.


page 4

page 5

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Due to a booming number of robotic applications that necessitates Autonomous Mobility Robots (AMR), an effective navigation approach that assures optimal robot motion to the desired position while avoiding obstacles is significantly important. Since in reality, the environment is often time-variant, the robot should also be able to handle dynamic obstacles and maximally stick to the shortest path. This paper is going to mainly focus on developing a motion planning scheme that allows the robot to travel in dynamic environment.

I-a Related Work

Navigation in dynamic environment is a popular topic in recent years. the state-of-art approaches adopt reinforcement learning to obtain the optimal policy that deals with moving obstacles

[1]. Mohanan et al.[2] divided commonly-seen navigation approaches into artificial potential field based, velocity based, probabilistic based etc., however, this classification can be further simplified to local based, global based, global local hybrid based algorithms.

Global navigation requires a map to run graphic search algorithms that output a collision-free path from the starter to the goal. In this case, A* and D* are two frequently used path planner. An algorithm that combines Depth-first Search (DFS) and Breadth-first Search (BFS) to achieve fast and optimal global path plan is proposed in [3]. In order to suit for time-evolving map, S. Koenig et al. proposed a incremental version of D* algorithm that automatically re-calculates global path once detecting environmental change[4]. Later the equivalent version of A* was proposed in [5] that continuously searches for the shortest path to handle pop-up obstacles. Nevertheless, digging into sophisticated real-time graphic search methods leads to unnecessary computational burden and may cause jerky movement if the re-plan is triggered too frequently, a simple but effective algorithm is still preferred.

A well-known local based navigation is Vector Field Histogram (VFH) approach proposed by J. Borenstein and Y. Koren

[6]. This algorithm is a refined version of potential field approach. It acquires sensory data to create obstacle polar histogram, from which the robot picks the optimal admissible valley to steer. Enhanced VFH was proposed by K Balan et al[7] where VFH is coupled with accessibility graph through Fuzzy Logic to obtain smoother motion and prevent local-minima. Other approaches include probabilistic mapping on the dynamics of the obstacle and calculates collision-free velocity for the robot[8]. Yet, planning entirely under local perspective may lose global optimality and often performs poorly when encountering situation like U-trap, narrow corridor, etc.

Based on the factors above, a local and global hybrid navigation strategy is more effective. The method adopted in ROS (Robot Operating System) is a cost-map based navigation that takes advantage of a global planner (Dijkstra) and a local planner (DWA) to achieve real-time updated navigation. The former is an efficient graphical search algorithm similar to A*, while the latter takes samples of all possible velocity command pair in a dynamic window and select the one that minimizes the cost function via forward propagation. However, this navigation scheme is sophisticated when deploying. Moreover, conflicts between the two planners could arise under certain circumstances.

I-B Contribution

This paper proposed a light-weight but robust hybrid robot navigation algorithm that combines VFH and A* suitable for time variant environment. The incorporation of VFH and A* for mobile robot navigation is novel in the field. Both simulation and hardware experiments show that our approach leads to successful and smooth mobile robot navigation. The global and local planner compensates for each other and there’s little chance for the planners to conflict with each other. The rest of the paper is organized as follows : Section II introduces the mathematical model of the robot kinematics and environment representation, Section III details the proposed algorithm, Section IV demonstrates and explains the results we obtained from both simulation and hardware test under various scenarios and Section V draws the conclusion accordingly.

Ii Modeling

We developed mathematical modeling for our robot and its surrounding environment:

Ii-a Environment Modeling

We apply probabilistic occupancy grid map implemented in [9]

to model the environment. This approach decouples the environment of given size (estimation of the actual field size) into multiple cells and assigns unique values to cells representing different state, namely, 0 for "free", 1 for "occupied" and -1 for "unexplored" (shown in Fig.

1 as an example). This map gets updated using laser scanner data. Introducing probabilistic methodology into mapping compensates for the odometry error and LiDAR scanning error resulting from wheel slippery and uneven ground, both of which cause accumulating offsets in the map.

Fig. 1: Occupancy Grid Mapping of the Testing Environment: pixels in while are free areas, pixels in black are obstacles and pixels in gray are unexplored

Ii-B Robot Modeling

Consider a planar mobile robot shown in Fig.2, a range scanner is mounted on top of the robot to detect surrounding obstacles and return the distance to the obstacle with respect to the robot’s local frame. The robot states representing its position and orientation expressed under the global frame is:

Fig. 2: Wheeled Robot Coordinate Frame Convention

To achieve trajectory tracking, given the instantaneous reference state , we adopted the error dynamics introduced in [10]:


Here, , and are the tracking errors to be eliminated. The controls of the robot are its linear velocity and the angular velocity which can be obtained through:


where and are the instantaneous desired linear and angular velocities, calculated as:


We adopt the non-linear control law verified and implemented in [10]:


where and are positive definite gains. Lastly, our robot is differential-drive, therefore the instantaneous radius of turning is determined by:


Iii Proposed Algorithm

The flowchart of the proposed dynamical obstacle avoidance algorithm is shown in Fig. 3.

Fig. 3: Dynamical Obstacle Avoidance & Navigation Flow Chart

A global motion plan is generated at the beginning of the navigation, providing a reference trajectory for the robot. If the tracking error is greater than a small enough threshold, motion planning will be re-executed. The robot will mostly governed by the non-linear control law stated in section II but keep reading the distance to the nearest obstacle from the LiDAR and switch to VFH speed command if the obstacle is too close.

The global motion plan sits on the prerequisite that the robot already possessed map of the environment and localized itself at . After the desired position and orientation has been specified by the user, the initial conditions (map, start and goal) are fed into the path planner where A* generates a collision-free path. Then the path is sent into the trajectory planner to obtain a function of time quintic trajectory. The reference velocity and acceleration are calculated by finding the first order and second order derivative of the trajectory accordingly. With the reference position, velocity and acceleration at each time sample, the instantaneous linear and angular velocity can be computed using Eqs. (7) - (8).

Iii-a Path Planning

Based on the environment modeling, we use A* path planner to search for a collision free path. A* takes the current pose, the goal pose, as well as the occupancy grid as inputs, computes an optimal path that does not pass through any obstacles. The resultant path is via-points represented by two vectors and :


Iii-B Trajectory Planning

The objective for trajectory planning is to find a polynomial time function to fit A* path. To find a balance between reasonable computation amount and a close fit to the path plan, we decided to use quintic polynomial:




the goal now is to find and . Firstly, we specify the end-time by which the robot should arrive at the goal according to how far is the robot from the goal:


where is the Cartesian distance from the robot’s current position to the goal position, is a positive time scaling factor. Given via-points assuming the robot travels along the trajectory at a constant speed, the time samples are:




Plugging time samples into the quintic function gives a matrix :


we can further define two vectors and that specify the via-points on the trajectory, as well as the desired horizontal and vertical velocities at these via-points:


Therefore, and can be calculated by:


and the quintic trajectory is obtained.

Iii-C Local Obstacle Avoidance

As shown in Fig. 3, VFH is triggered when the robot is too close to the obstacle. Yet, in order to maximally stay on the desired trajectory, at any time instance and robot configuration , , at , the target position for VFH under global frame should be , . Converting this position to robot frame gives current goal position , with respect to local frame:


then the target direction for VFH is:


The robot’s velocity need to be determined according to the steering direction. For the purpose of smoothing the robot’s movement, we set its linear velocity as a constant (0.2 [m/s] for our case), and only change its angular velocity.

Fig. 4: Angular Velocity Calculation

In determining the angular velocity, consider a look-ahead point along the steering direction, 1-unit distance away from the origin of the robot local frame, as is shown in the figure above, where is the steering direction. We intend to find an arc that links the local frame origin to the look-ahead point as the robot’s desired trajectory. It is easily known from the graph that:


where is the arc radius. Therefore:


Again, we assume the robot to be differential steered, therefore by integrating Equ. (9) into Equ. (28) we obtain:


During implementing the VFH algorithm, we notice drastic oscillation occurs. To smooth the movement of the robot, we apply a first order IIR filter to reduce the high frequency change in robot’s angular velocity. The difference equation of the filter is given by:


where is the last angular velocity, is the new angular velocity command, is the new angular velocity to be sent to the robot.

Iv Results

Simulation and hardware testing are performed to validate the proposed algorithm and compare with counterpart navigation scheme from ROS navigation package. The hardware used for experiment is a Jackal UGV mounted with Hokuyo LiDAR and on board NUC (see Fig.5). The robot localizes itself using the EKF fused data from the wheel encoder and the embedded IMU sensor. The Hokuyo LiDAR on top of the robot provides 270 degree scanning range and returns a 1080 by 1 matrix indicating distance to the obstacles at every 0.25 degree with respect to local axis.

Fig. 5: Jackal UGV hardware setup

Iv-a Algorithm Verification

We tested the trajectory tracking control law in a customized Gazebo world simulator. Taking the A* path as input, the trajectory plan is demonstrated in Fig. 6 where the trajectory is shown on top of the A* path for comparison purpose. The desired linear and angular velocity can be derived from trajectory using Eqs. (5) - (6).

Fig. 6: Trajectory Plan Examples ((a) and (b) show two difference environment, obstacles are not displayed here)

Fig. 7 shows the robot executing the planned trajectory in a static environment. Here, the obstacles are marked with black dots. One can tell that with the global motion planner alone, the robot is able to safely navigate itself to the goal.

Fig. 7: Trajectory Execution Examples: obstacles are black dots, the blue curve shows the recorded robot position, the red curve shows the A* path

Fig.8 shows an example of VFH kicks in when the robot detects the obstacle that didn’t appear during map building.

Fig. 8: Avoiding Pop-up Obstacle with VFH: (a) shows the robot recorded position and the A* path; The while box in (b) is the pop-up obstacle deployed after mapping the environment

Nevertheless, after switching to VFH, the tracking errors will start growing. When the errors get too large and the robot is still not able to get rid of the obstacle, the motion planner will be triggered again. (see Fig.9)

Fig. 9: Motion Replan Triggered Examples (the blue curve is the recorded robot position, the green curve is the new path plan based on the latest environment info)

We tested our algorithm on the Jackal UGV platform both in static and dynamic environment (see Fig.10). The static environment consisted of walls in a U-shape. They robot needs to travel from one dead end to the other, while in the dynamic environment, the tester kept walking in the field to block the robot which was traveling from one corner to another.

Fig. 10: Hardware Experiment Setup: (a) shows the operator moves to block the robot, (b) shows the robot navigating through a static environment

The results obtained in static environment is shown in Fig.11. We can tell from the robot’s trajectory that it mostly followed the A* path plan without having VFH kicked in and no re-plan was triggered as well. The errors also dropped to a trivial level as the robot reached the goal.

Fig. 11: Hardware Experiment in Static Environment: (a) shows the recorded robot position inside the map, (b) shows the errors vs. time

For dynamic environment testing, the robot first executed global motion plan without the operator’s presence. When the robot started moving, the operator entered the field and tried repeatedly to block the robot (see Fig.12). From the results, we can tell re-plan was triggered from the discrepency in the robot’s trajectory, and there are slight fluctuation in the errors due to the operator’s blocking behavior. However, the robot successfully arrived at the goal eventually.

Fig. 12: Hardware Experiment in Dynamic Environment: (a) shows the recorded robot position inside the map, (b) shows the errors vs. time

Iv-B Comparing with ROS NAV-STACK

We compared our proposed navigation scheme with the ROS navigation stack. The field setup for comparison is seen in Fig.13. The results are demonstrated in Fig.14 and Fig.15.

Fig. 13: (a) Experiment field setup; (b) simulation field setup
Fig. 14: Simulation comparison; (a) ROS NAV-STACK global plan; (b) VFH-A* global plan
Fig. 15: Experiment comparison: (a) ROS NAV-STACK global plan; (b) VFH-A* global plan

The robot’s running time is shown in Table I.

Simulation 43 [sec] 60.98 [sec]
Experiment 48.87 [sec] 36.31 [sec]
TABLE I: Run-time Comparison

Because of wider space and better conditioned map in Gazebo simulation, though the global planners gave similar path, ROS NAV-STACK is able to drive the robot faster thanks to the nature of DWA. Yet when it comes to experiment tests using ROS NAV-STACK, robot stuck at the hairpin turn for several seconds due to the "local minima" issue discussed above. Our proposed method does not seek to maximally track the global path and is able to cope with poor-conditioned map resulting from inaccurate robot localization using VFH. As a result, VFH-A* drove the robot faster in hardware experiment. It is worth noticing that the VFH’s linear velocity used in both simulation and experiment are the same, and is set relatively low for safety, therefore the run-time in simulation has great improving room.

V Conclusion and Discussion

In this paper, we proposed a hybrid navigation scheme for mobile robot that is able to navigate through dynamic environment. It can be concluded that this approach successfully works under various situations both in simulation and hardware test, and drives the robot on a smooth path. It is light-weight, fast, capable of handling pop-up and moving obstacles while maintaining the robot on the optimal path. Comparing to the widely adopted ROS navigation package, there is less chance in the proposed navigation algorithm where the global and local planner conflict, causing to oscillation in the robot motion.

As future work, the algorithm can be further enhanced by optimizing the trajectory to better fit the A* path (e.g adopting spline interpolation between via-points) and seeking to achieve higher velocity to lessen travel time. Tests are needed under more complicated environments (e.g. consider the effect of uneven ground and map the arena in 3D) to show the algorithm’s robustness.


  • [1] M. Everett, Y. F. Chen and J. P. How, "Motion planning among dynamic, decision-making agents with deep reinforcement learning," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 3052-3059
  • [2] M. Mohanan and A. Salgoankar, “A survey of robotic motion planning in dynamic environments,” Robotics and Autonomous Systems, vol. 100, pp. 171–185, 2018.
  • [3]

    P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,”

    IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.
  • [4] S. Koenig and M. Likhachev, “D* lite,” Aaai/iaai, vol. 15, 2002.
  • [5] S. Koenig, M. Likhachev, and D. Furcy, “Lifelong planning A*,” Artificial Intelligence, vol. 155, no. 1-2, pp. 93–146, 2004.
  • [6] J. Borenstein, Y. Koren et al., “The vector field histogram-fast obstacle avoidance for mobile robots,” IEEE Transactions on Robotics and Automation, vol. 7, no. 3, pp. 278–288, 1991.
  • [7] K. Balan, M. P. Manuel, M. Faied, M. Krishnan, and M. Santora, “A fuzzy based accessibility model for disaster environment,” 2019 International Conference on Robotics and Automation (ICRA), 2019, pp. 2304–2310.
  • [8] C. Fulgenzi, A. Spalanzani, and C. Laugier, “Dynamic obstacle avoidance in uncertain environment combining pvos and occupancy grid,” Proceedings 2007 IEEE International Conference on Robotics and Automation, 2007, pp. 1610–1616.
  • [9] E. Kaufman, T. Lee, and Z. Ai, “Autonomous exploration by expected information gain from probabilistic occupancy grid mapping,” 2016 IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR), 2016, pp. 246–251.
  • [10] K. L. Besseghieur, R. Tr˛ebi´nski, W. Kaczmarek, and J. Panasiuk, “Trajectory tracking control for a nonholonomic mobile robot under ros,” Journal of Physics: Conference Series, vol. 1016, no. 1, 2018, p. 012008.