Path Planning
这一部分利用RPM算法完成路径规划，即在地图上随机产生点，去掉不合法的点和边后算最短路，再用贝塞尔平滑。
整体分为3步：
1.产生随机点，去掉不合法路径后建图
2.计算最短路
3.平滑
建图和最短路
建图很简单，用均匀分布在已探索过的区域中产生随机点，每个点保留欧式距离最近的几个点连边，检查边的合法性，完成建图。最短路也没什么好说的，因为剩下的边和点都不多，只有几百，什么最短路算法都行(甚至Floyd都行)，我用的是spfa，简单又好写。代码见path_planning.py和c_extern/find_path.cpp，主要函数在cpp中。
平滑
用最短路得到的是一段折线，我们当然希望能平滑一下。直接使用n阶贝塞尔计算量太大，而且不过路径上的点，可能会出现偏差然后撞障碍物上了。我们也试过用三阶B-样条，但是也有相同的问题，而且连起点和终点都不过，对后面的控制器设计带来麻烦。最后我们采用了贝塞尔插值，具体参考https://www.cnblogs.com/muxue/archive/2010/06/23/1763886.html 。文中是对封闭多边形平滑，我们需要对折线平滑，区别就是起点和终点，我们直接用二阶贝塞尔，代码见path_planning.py。