The planning stack is responsible for generating the path and trajectory for the robot. The planner would be taking as input the initial position of the robot, the goal position, the map and the odometry and would be giving the computed velocity commands as output. We plan to implement our planner as a plugin that works with the move_base_flex framework. We are using RRT* to plan a path through the environment.
To know more about these, click the links below
FVD Implementation for Planning
The planner subsystem takes as input the robot odometry (from the SLAM subsystem), the global and local maps, the goal point given by the user, and most importantly, the robot parameters that are defined by the user as per the robot’s capabilities. The global map from the SLAM subsystem is first converted into a 2.5D elevation map. Then the traversability map generator uses the robot parameters and applies a series of filters that to this 2.5D elevation map to get the traversability map. This traversability map is fed to the RRT* planner that then generates the path through the environment. The path generated reasons about the abilities of the robot.
After the path is generated, the local planner generates velocity commands for the robot, using the odometry as feedback. It can also be thought of as a high level controller. As the robot moves along the path, the local planner might discover new obstacles using the provided local map. If it discovers new obstacles, once again a check for the negotiability of the new obstacle is performed and the hardware agnostic path is modified accordingly. If no new obstacles are found, the robot continues following the old hardware agnostic path. Both the hardware agnostic global planner as well as local planner report any failures to plan a feasible path for the given number of iterations allowed in RRT*. The local planner also notifies the user if the robot has reached the goal location.
A RRT* planner is implemented in C++ as a Move Base Flex-compatible plugin. The planner uses a space-based path shortening method to try to converge to an optimal path with increasing number of iterations. This is all run in simulation with user inputs of the goal point and robot parameters (as mentioned prior: FSOH, MTS, gradient influence, and robot radius). A visualization of the simulated environment can be seen in Figure 14, this shows the traversability map, path generated, and the obstacles generated. A custom GUI was created and integrated to modify and create simulated maps and robot parameters.
The results are shown for different test cases upon which the planner was initially developed. These test cases allowed for the implementation to include edge cases where the planner could potentially fail, and apply mitigation strategies (such as running the planner for longer iterations). Lastly, the unit tests served the purposed of debugging minor issues in the planner that were not easily detected during implementation and in overall subsystem tests.
For analysis, we included an option for verbose output in the planner to get the details of each step the planning algorithm was taking. Additionally, the planner also outputs the total time taken as well as the cost of the generated plan for analysis. In our analysis, we found that the planner starts slowing down significantly if the number of iterations go over 2000. However, keeping the iterations below 1200 gave a lot of sub-optimal paths. Hence we decided the number of iterations to be 1500. We also tested the effect of neighborhood size, and the results were similar to the number of iterations; very slow planning for higher neighborhood sizes and sub-optimal results in case of low neighborhood sizes. The sweet spot in this case was 4.0 meters.