6.4.1 迭代二次启发式规划算法推导