最近在学习路径规划，想尝试优化路径。通过阅读文献，发现对于优化路径这件事，有许多人用过一种去冗余点的方法，但CSDN上没有人写过。所以写在这里，一来记录，二来分享。
**提出问题：**如下图，规划后的路径包含许多弯曲的地方，而中间有些弯曲的地方完全可以改为直线。那么如何改进呢？
思路：将第一个点和第二个点连接起来，如果二者之间没有障碍物，那么就不管第二个点了。再将第一个点和第三个点连接起来，如果二者之间没有障碍物，就不管第三个点了。再把第一个点和第四个点连接起来。。。。直到第一个点和与他连接的点之间出现障碍物为止。
举例解释：在上图中，将第一个点依次尝试和后边的点连接，当连接到第六个点的时候，发现①和⑥之间出现了障碍物，那就不要再连了，退回去，把①⑤之间的线段作为剪枝成功的一段路径。
下一步：把⑤再依次和后边的点连起来，重复上述步骤。
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];
```cpp
在这里插入代码片
