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. Mohanan et al. 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 . 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. Later the equivalent version of A* was proposed in  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. 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 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. 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.
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.
We developed mathematical modeling for our robot and its surrounding environment:
Ii-a Environment Modeling
We apply probabilistic occupancy grid map implemented in 
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.
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:
To achieve trajectory tracking, given the instantaneous reference state , we adopted the error dynamics introduced in :
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 :
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.
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.
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:
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.
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.
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. 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.8 shows an example of VFH kicks in when the robot detects the obstacle that didn’t appear during map building.
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)
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.
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.
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.
Iv-B Comparing with ROS NAV-STACK
The robot’s running time is shown in Table I.
|Simulation||43 [sec]||60.98 [sec]|
|Experiment||48.87 [sec]||36.31 [sec]|
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.
-  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
-  M. Mohanan and A. Salgoankar, “A survey of robotic motion planning in dynamic environments,” Robotics and Autonomous Systems, vol. 100, pp. 171–185, 2018.
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.
-  S. Koenig and M. Likhachev, “D* lite,” Aaai/iaai, vol. 15, 2002.
-  S. Koenig, M. Likhachev, and D. Furcy, “Lifelong planning A*,” Artificial Intelligence, vol. 155, no. 1-2, pp. 93–146, 2004.
-  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.
-  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.
-  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.
-  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.
-  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.