add pso planning algorithm