路径优化：去除路径冗余点/路径拉伸/路径剪枝

最近在学习路径规划，想尝试优化路径。通过阅读文献，发现对于优化路径这件事，有许多人用过一种去冗余点的方法，但优快云上没有人写过。所以写在这里，一来记录，二来分享。

提出问题:如下图，原始路径包含许多弯曲很小的地方，有些地方完全可以改为直线。那么如何改进呢？

思路：将第一个点和第二个点连接起来，如果二者之间没有障碍物，那么就不管第二个点了。再将第一个点和第三个点连接起来，如果二者之间没有障碍物，就不管第三个点了。再把第一个点和第四个点连接起来。。。。直到第一个点和与他连接的点之间出现障碍物为止。

举例解释：在上图中，将第一个点依次尝试和后边的点连接，当连接到第六个点的时候，发现①和⑥之间出现了障碍物，那就不要再连了，退回去，把①⑤之间的线段作为剪枝成功的一段路径。
下一步：把⑤再依次和后边的点连起来，重复上述步骤。

MATLAB代码：

function [directcost,beacon_coo]=PathOptimization(trace_coo,map,row,col,safe_dis)
beacon_coo=trace_coo(end,:);%
S_coo=beacon_coo(1,:);%
k=size(trace_coo,1);%
while k~=1
    E_coo=trace_coo(k-1,:);
    node1=S_coo;
    node2=E_coo;
    if checkConstraint(node1,node2,map,row,col,safe_dis)==1%不在障碍上
        beacon_coo=[trace_coo(k,:);beacon_coo];
        S_coo=beacon_coo(1,:);
        k=k+1;
    end
    k=k-1;
end
beacon_coo=[trace_coo(1,:);beacon_coo];

其中，trace_coo是保存了n个坐标的n行2列矩阵。beacon_coo亦是如此。