Collision Distance Detection Based on Swept Volume Strategy for Optimal Motion Plan