5.3.2 非完整路径规划问题的求解