ROS：一种路径优化方法

此处提供一种路径优化方法

名字：拉直法，即将路径拉直，将路径上一些不在直线上的点删掉。取直线的原则为：不能与障碍物相撞。

需要参考代码者请留言写上邮箱地址。

算法说明：

如图：

1、设路径点为数组P[end],end为一个大于2的整数。
2、从路径一个点P[x]开始尝试与路径上其他的点P[x+2]～P[n]～P[end]拉直，若能拉直则删除Px与Pn之间所有的路径点（不包括Px和Pn）。
3、x取值范围[0,end-3]，依次增大，每次加1，x每加1需要与n为[x+2,end]上所有的点做拉直计算。
4、n的取值范围[x+2,end]。
5、当x >= end-2，算法结束。

效果：