B站老王笔记-2-4(中)路径二次规划算法

老王说，听这个很简单，但编程要很细节

理论很简单，要关注矩阵的规模，子矩阵的

第一遍

听着感觉也不简单的’

开辟凸空间，即当坐标（s,l)的s在s1处时，l在lmax1与lmin1，之间选取。

即

三个大矩阵的行列

将不等式约束写成矩阵形式，但是考虑到车有体积所以不能简单地将汽车近似成一个质点，需要考虑汽车四个角的约束，几何关系如下。

将四个角限制到最小值内

同理下面的最大值也一样

不等式约束就可以写成大于最小值，小于最大值

化成矩阵

总的不等式约束

写出代价函数

和前面1-3的公式结合起来进行坐标转换，完成路径规划的算法