This article is focused on the problematics of path planning, which means finding the optimal path between two points in a known environment with obstacles. The proposed path-planning method uses the wavefront algorithm, and two modifications are implemented and verified. The first modification is the removal of redundant waypoints. The first modification is applied because the wavefront algorithm generates redundant waypoints. These waypoints cause unnecessary changes in the direction of movement. The second one is smoothing the generated trajectory using B-spline curves. The reason for applying the second modification is that trajectory generated by the wavefront algorithm is in the form of the polyline, which is inadequate in terms of the smoothness of the robot’s motion. The verification of the proposed method is performed in environments with different densities of obstacles compared with standard Dijkstra’s and A* algorithms.