5、Optimization Inside Motion Planning
- 动态规划来自于动态系统， 通过类似于有限元的方式，把问题抽象再离散空间里面，把重复计算通过aggregating的方式进行简化。
问题：计算时长太长，
，这么撒点太复杂。
- 对于凸问题，或者单调问题，求最优解，用binary search搜索某个点的值就可以，收敛速度是指数收敛。
- 牛顿法更快，考虑了不同点位置的斜率变化率大小，收敛速度是指数平方，二次收敛。本质是二阶逼近的泰勒展开。
- 牛顿法的思想：把函数用来泰勒展开然后逼近，获得更多的信息，用更快的速度去收敛最优解。
- 二次规划的核心，如果知道convex problem的二阶导数，可以更快的找到最优解，本质和taylor expansion和牛顿法相同。
二次规划有时候会处理高纬度问题，问题是在多维的空间里，进行优化convex optimization的时候，很难找到一个高阶的类似hessian矩阵的东西，一维的时候偏导是参数的个数，二维是参数个数的平方，三维是个tensor，非常庞大。这是个权衡的过程，实践中二维就够用了。
Quadratic Programming的劣势：
牛顿法要求derivative严格单调递增，一般的函数不是这样子的。
动态规划要求打点很多，对于高维是灾难，牛顿法只能找到局部最优，也不行。
解决方法：divide and conquer
动态规划 + 牛顿法 二次规划 可以找到global 的最优解。
如上图找到第一个或第二个点，这个点作为牛顿法的起始点，从这个点做一个hot start，然后做一个QP problem。这个时候有大概率找到global optima。EM Planner里面的组合优化，启发式的搜索，DP撒点不要太密也不要太稀疏， DP找到hot start然后QP找到最优解。启发式搜索只要reward function 不是特别复杂都可以找到最优解，其他类似方法有模拟退火等。
二次规划
-
QP问题，没有constraint时，直接求导就行。
-
QP问题，有constraint。
二次规划找局部值或者边界值，hit boudary的constraint 叫 active constraint。在求解带约束的条件的时候，看一下满足active constraint的情况下的最优解，不满足情况下的最优解的样子，看不满足active constraint情况下的最优解在不在约束内，在的话就是最优解，不在的话看满足active constraint情况下的最优解。这种思想就 active set methode。
拓展到高维后的形式：
的求解
牛顿消元法：QR，LR算法。拆解为一个上三角矩阵或正交矩阵，这样容易计算。
同时这个矩阵也是个symmetric strictly positive definite。对称矩阵有更好的decompostion的方式：比如cholesky decomposition，schur decomposition。可以很快的求解线性系统。
-
QP问题，有等式约束
把下面的等式带入上面的function还元，不过很慢。
有更快的方法，lagrangian方法，dual parameters类似松弛变量，这种情况下等式就是一个超平面。
这个和之前的类似，当然也可能有没有解的情况。每加一个等式的约束，可行空间就降一个维度。
-
QP问题，有inequality constraints
重点是12.30e，不等式为0，active的时候lambda为任意值，不等式不为0是，lambda为0。
除了active set methode以外还有其他方法，在松弛变量上做文章，interior point methode。
先当成没有inequality的去算，之后看那些inequality没有满足，这个时候这个条件被加入到active set里面。即这个条件等于0。重复直到找到最优解。
目标函数不是二次型的凸函数怎么办？
比如
，局部用牛顿法逼近，用surrogate的objective function去代替原来的function。sequentially解决问题。在高维空间求解hessian。有很多方法：BFGS，LBFPS
推介书目：
Stephen Boyd convex optimization
Stephen J. Wright Numerical Optimization