The traditional TEB algorithm is prone to abnormal behaviors such as backsliding and large steering in the messy scene planning, which causes acceleration jumps, unsmooth control commands, and large impacts on the robot, which is not conducive to the trajectory tracking of the mobile robot. This paper proposes an improved TEB algorithm, which can plan a safer motion trajectory by adding the hazard penalty factor constraint,reduce the maximum impact in motion by adding the acceleration jump suppression constraint, and reduce the end impact by adding the smooth ends constraint, so as to achieve the smooth and accurate arrival of the target point. Then construct a graph optimization problem, take the pose and time interval of the robot as the nodes, the objective function and the constraint function as the edges, and use the sparsity of the problem to quickly obtain the control amount at the corresponding time point. Finally, the performance of the proposed improved TEB algorithm is verified by a large number of comparison tests of robot operating system simulation and physical experiments of real differential robot planning algorithm. The results show that the improved TEB algorithm can plan safer and smoother trajectory in complex environment, reduce the impact of the robot, and realize more reasonable movement of the robot.