学习记录-基于采样的路径规划算法
内容来源
记录学习深蓝路径规划课程-基于采样的路径规划一节的作业和笔记,实现基于matlab的RRT以及RRT*算法实现以及可视化。
RRT
- 伪代码
- 树结构
x, y记录此节点位置
xPrev, yPrev记录父节点位置
dist记录此节点到起点的距离(与作业源码不符,我自己进行了修改)
indPrev记录父节点在树中的索引
主要步骤
- 采样:Sample
%Step 1:在地图中随机采样一个点x_rand
%提示用(x_rand(1), x_rand(2))表示环境中采样点的坐标
x_rand = randi(800, 1, 2); % 全局随机采样
- 搜索邻近节点:Near
function [x_near, near_Idx] = Near(x_rand, T)
% 在树T中搜索距离随机点x_rand最近的节点,返回它及其它在树中的索引
count = size(T.v,2);
min_dis = 10000;
for node = 1: count
dis = sqrt(power((T.v(node).x-x_rand(1)) ,2) + power((T.v(node).y - x_rand(2)), 2) );
if dis<min_dis
min_dis = dis;
near_Idx = node;
x_near(1) = T.v(node).x;
x_near(2) = T.v(node).y;
end
end
end
- 生成新节点:Steer
function x_new = Steer(x_rand, x_near, StepSize)
% 将距离随机点x_rand最近的节点x_near在x_rand方向上平移StepSize的距离,生成新节点x_new
dis = distance(x_near, x_rand);
% 强迫症,想让新节点坐标为整数,fix 舍余取整(也可不取整数)
x_new(1) = fix(((dis-StepSize)*x_near(1) + StepSize*x_rand(1)) / dis);
x_new(2) = fix(((dis-StepSize)*x_near