基于贝赛尔曲线的轨迹优化： 前端：Astar寻径 后端：运动走廊膨胀＋Bezier
1)前端：Astar寻径 前端path finding使用的是经典的Astar算法，唯一不同的是在搜索的时候对每个node新增了一个与障碍物距离的代价指标，它与启发式代价和累计代价一起决定了node在openlist中的排序。
2)后端：运动走廊膨胀＋Bezier优化
①为了得到比较稀疏的运动走廊，膨胀分为两步。第一是基于前端得到的path节点进行膨胀，但是加了一个限制条件，即已经被bounding box覆盖的节点不予膨胀，这样的好处是不用再扩展冗余的box以及为了下一步的简化减少了很大的计算量。第二是对得到的bounding box进一步简化，因为前一步得到的bounding box仍然存在大量重叠的情况，对接下来的轨迹优化来说无疑是无用且消耗计算的，这里简化的核心**是从当前boxNow开始按照顺序找到其不相交的boxA，并将该boxA的前一个boxB保留下来，并删除从boxNow到boxB之间的所有box,然后boxA作为新的boxNow继续删减。
②Bezier优化主要参考资料为《Online Safe Trajectory Generation For Quadrotors Using Fast Marching Method and Bernstein Basis Polynomial》、《Minimum snap trajectory generation and control for quadrotors》、https://www.bilibili.com/video/BV1hs4115713
add: 代码中使用了二次规划库(qpOASES)以及矩阵库(Eigen)
代码在持续优化中...欢迎大家一起交流。 在此基础上可以加入具体机器人运动模型和控制算法进行轨迹追踪模拟...