繪制最優(yōu)路徑

從目標(biāo)點(diǎn)開始，依次根據(jù)節(jié)點(diǎn)及父節(jié)點(diǎn)回推規(guī)劃的路徑直至起點(diǎn)，要注意tree結(jié)構(gòu)體中parent的長度比child要小1。最后將規(guī)劃的路徑顯示在figure中。

%% 繪制最優(yōu)路徑
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);

程序運(yùn)行最終效果如下：

紅點(diǎn)都是生成點(diǎn)隨機(jī)點(diǎn)，綠點(diǎn)是tree中節(jié)點(diǎn)，紅色路徑即為RRT算法規(guī)劃的路徑。
路徑平滑（B樣條曲線）

由于規(guī)劃的路徑都是線段連接，在節(jié)點(diǎn)處路徑不平滑，這也是RRT算法的弊端之一。一般來說軌跡平滑的方法有很多種，類似于貝塞爾曲線，B樣條曲線等。

我在這采用B樣條曲線對規(guī)劃的路徑進(jìn)行平滑處理，具體的方法和原理我后續(xù)有時(shí)間再進(jìn)行說明，這里先給出結(jié)果：

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