In this paper, an algorithm is presented for a swarm motion stability based on Lagrange equations. Having virtual energy sources, the mentioned algorithm is designed and applying energy values to Lagrange equations the swarm members move toward the desired target point. Moreover, the presented stability algorithm has been generalized to prevent collision between members of the swarm as well as avoid collision with obstacles and two repulsive operators have been designed to guaranty the safety of the swarm members along the path. Arriving of all members at the target point, an aggregation switch is designed for members’ aggregation. Prioritizing on experimental works, this feature enables the swarm members move more freely and cover a greater area along the path. It is also possible to define and arrange virtual obstacles in any desired layouts (e.g., narrow corridor) in order to achieve different goals such as avoiding an area, guiding the swarm in the desired direction, etc. the presented algorithm is suitable for identification, navigation, search and rescue, protection, etc. Using MATLAB software this algorithm is simulated and validated for the movement of swarm robots in both two- and three-dimensional spaces covering different conditions. The simulation results demonstrate the capabilities and efficiency of the presented algorithm for application in swarm motion.