Matlab中如何绘制最优路径

绘制最优路径

从目标点开始，依次根据节点及父节点回推规划的路径直至起点，要注意tree结构体中parent的长度比child要小1。最后将规划的路径显示在figure中。

%% 绘制最优路径
temp = tree.parent(end,:);
trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];
for i=size(tree.child,1):-1:2
    if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp)
        temp = tree.parent(i-1,:);
        trajectory(end+1,:) = tree.child(i,:);
    if(temp == x_start)
        trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution];
    end
    end
end
plot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);
pause(2);

程序运行最终效果如下：

红点都是生成点随机点，绿点是tree中节点，红色路径即为RRT算法规划的路径。
路径平滑（B样条曲线）

由于规划的路径都是线段连接，在节点处路径不平滑，这也是RRT算法的弊端之一。一般来说轨迹平滑的方法有很多种，类似于贝塞尔曲线，B样条曲线等。

我在这采用B样条曲线对规划的路径进行平滑处理，具体的方法和原理我后续有时间再进行说明，这里先给出结果：

黑色曲线即位平滑处理后的路径。