4.optimization inside motion planning
优化问题的核心，有三点：1是objective function ,objective function是通过一些平滑性等方面指标的一个定义，是一个合集2是constraint 如何抽象，包括smoothing的constraint，dynamic的constraint以及一些交通规则3,solver约束问题下的最优解方法：动态规划，通过有限元的方法将整个空间离散化，把重复计算通过aggregating的方法进行简化，二次规划
但是动态规划的时间复杂度仍然较高---采用牛顿，收敛速度更快
优化问题：牛顿迭代法，牛顿迭代法就是相当于泰勒的二阶展开去逼近，收敛速度是阶乘的平方，但是牛顿法要求一阶导数为单调递增，三阶导数为正值，凸的问题。二次规划就是运用牛顿法的思想
对于上图这种情况，直接用牛顿法可能会求出两个局部最优解，取决于初值点的选取，求不出来全局的最优解，用动态规划也只能求出来局部最优解，那怎么办？--分段求解，可以先用动态规划对系统有一个大致的了解，然后再细致去求最优解。如何上图，分段以后，可以看到，0.2与0.4时，最小，然后再利用牛顿法在0.2与0.4之间求解即可以很快求出最优解。
在做决策规划时，比如前面有一个障碍物，决策就是决定从障碍物左边还是右边绕过比较好，然后就可以利用动态规划，做一个大致了解，比如用动态规划求出来左边比较好，然后就变成一个凸问题，再利用牛顿法在障碍物的左边求出来最优解。
二次规划：
对于带等式约束的二次规划问题求解：
加入拉格朗日算子，这样仍旧是quadratic programming,对于二次规划，最优解就是在导数为零的地方。
对于不等式约束的问题：active set method
总结：
无人车的最优化问题本身是一个非线性的，非凸的，比如绕障问题，从左边走，还是从右边走，利用启发式方法，先用动态规划给出一个粗略的了解，大概知道走右边比较好，然后再给右边用一个quadratic programming的方式。构造出一个凸空间，在这个凸空间里面去寻找最优解。
推荐书籍：