繪制最優路徑

從目標點開始，依次根據節點及父節點回推規劃的路徑直至起點，要注意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樣條曲線對規劃的路徑進行平滑處理，具體的方法和原理我后續有時間再進行說明，這里先給出結果：

黑色曲線即位平滑處理后的路徑。