Trajectory Optimization with Particle Swarm Optimization for Manipulator Motion Planning

Optimization-based methods have been recently proposed to solve motion planning problems with complex constraints. Previous methods have used optimization methods that may converge to a local minimum. In this study, particle swarm optimization (PSO) is proposed for trajectory optimization. PSO is a population-based stochastic global optimization method inspired by group behaviors in wildlife and has the advantages of simplicity and fast convergence. Trajectory modifications are encoded in particles that are optimized with PSO. The normalized step cost (NSC) concept is used for the initialization of the particles in PSO. A method for reusing previously optimized parameters is also developed. The optimized parameters are stored together with corresponding NSC vectors, and when a constraint violation occurs, the parameters associated with an NSC vector that is similar to the query NSC vector are selected. The selected vector is used for initializing the particles. The reuse of the previously optimized parameters improves the convergence of the PSO in motion planning. The effectiveness of these methods is shown with simulations and an experiment using a threedimensional problem with constraints. The proposed algorithm successfully optimized a trajectory while satisfying the constraints and is less likely to converge to a local minimum.