五点平滑算法轨迹优化

本代码用于平滑轨迹，方法是每进入5个点，就对五个点进行处理，让其符合一段平滑曲线。前一个第五点后面新进的第一个点的链接方法是直接连线（暂未想到更好的方法，如果有就再次更新），里面有注释可以慢慢看。