Time elastic band algorithm(Rosmann,2012) solves the local planning problem using a weighted multi-objective optimization framwork. Then the optimization is transformed into a hypergraph and solved with G2O. The TEB algorithm is avalible as a ROS package developed by Rosmann, you can download the whole package here. To make the trajectory produced by the TEB algorithm less jerky, an item considering jerk (derivative of acceleration) is added to the original optimazation framwork.
The modified hypergraph is shown below.
- ROS kinetic
- Download the original teb package
- Add the edge_jerk.h file in the /include/teb_local_planner/g2o_types
- Replace the optimal_planner.h and teb_config.h in the /include/teb_local_planner
- Replace the optimal_planner.cpp and teb_config.cpp in the /src