基于matlab的RRT&RRT*算法实现以及可视化

内容来源

记录学习深蓝路径规划课程-基于采样的路径规划一节的作业和笔记，实现基于matlab的RRT以及RRT*算法实现以及可视化。

RRT

1. 伪代码
   
2. 树结构
   x, y记录此节点位置
   xPrev, yPrev记录父节点位置
   dist记录此节点到起点的距离(与作业源码不符，我自己进行了修改)
   indPrev记录父节点在树中的索引
   

主要步骤

1. 采样：Sample

    %Step 1:在地图中随机采样一个点x_rand
    %提示用（x_rand(1), x_rand(2)）表示环境中采样点的坐标
	x_rand = randi(800, 1, 2);  % 全局随机采样

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

3. 生成新节点：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(2) + StepSize*x_rand(2)) / dis);
end

4. 碰撞检测：collisionChecking（作业源码中已给-基于给定的环境图片设计的）

function feasible=collisionChecking(startPose,goalPose,map)
% 输入为两节点坐标和地图信息，若两节点直线连接不会经过障碍物，则返回true, 碰到障碍物则为返回false
feasible