基于凸优化的车辆运动规划
工具：matlab、cvx（凸优化工具箱）
初始可行解
然后出现障碍物后
这是根据（时间差）每次求出的路径
最终得出的新的轨迹。
对于多个车辆（n大于等于2）时
将避障条件变为
即每辆车之间要满足这个条件：
这是两个车初始路径。
最后优化轨迹：
第一辆车从（2,5）到（8,5），第一辆车从（5.2）到（5.8），依次对应每个点。
本文优化目标只用了距离，同时可以加时间模型和角度变化等模型，加权起来求最优。约束只加了位置和速度约束，同时可以加上加速度的约束，以及地形约束等。