Local path planning is a necessary ability for mobile robot navigation, but existing planners are not sufficiently effective at dynamic obstacle avoidance. In this article, an improved timed elastic band (TEB) planner based on the requirements of mobile robot navigation in dynamic environments is proposed. The dynamic obstacle velocities and TEB poses are fully integrated through two-dimensional (2D) lidar and multi-obstacle tracking. First, background point filtering and clustering are performed on the lidar points to obtain obstacle clusters. Then, we calculate the data association matrix of the obstacle clusters of the current and previous frame so that the clusters can be matched. Thirdly, a Kalman filter is adopted to track clusters and obtain the optimal estimates of their velocities. Finally, the TEB poses and obstacle velocities are associated: we predict the obstacle position corresponding to the TEB pose through the detected obstacle velocity and add this constraint to the corresponding TEB pose vertex. Then, a pose sequence considering the future positions of obstacles is obtained through a graph optimization algorithm. Compared with the original TEB, our method reduces the total running time by 22.87%, reduces the running distance by 19.23%, and increases the success rate by 21.05%. Simulations and experiments indicate that the improved TEB enables robots to efficiently avoid dynamic obstacles and reach the goal as quickly as possible.
Read full abstract