Finding Better Robot Trajectory by Linear Constrained Quadratic Programming

Finding feasible motion for robots with high-dimensional configuration space is a fundamental problem in robotics. Sampling-based motion planning (SBMP) algorithms have been shown to be effective for these high-dimensional systems. But the biggest flaw of SBMP methods is that the trajectory is a combination of multiple linear paths under configuration space, which causes a lot of unnecessary acceleration and jerk. So how to optimize the solution of SBMPs efficiently is a key to improve the robot trajectory quality. In this paper, a robot trajectory optimization method based on linear constrained quadratic programming is proposed, which only need collision query, no distance or penetration calculations, and no prior knowledge of the environment. We use a series of simulation to prove the effectiveness and correctness of the methods.