informed-RRT*(RRT Family)全局最优轨迹规划算法 # 机器人轨迹规划(路径规划) #RRT #RRT-Connect 双向RRT #RRT* # iRRT* //. MATLAB coding //. 可快速自定义不同维度的栅格地图; //. code模块化编写&编程规范可有助于和进一步 扩展和创新以及与其它算法联仿 ! ! (RRT family): 1. RRT code; 2. RRT_Connect code; 3. RRT* code; 4. informed-RRT* code;

在机器人领域，轨迹规划（也就是路径规划）一直是个热门且关键的话题。今天咱们就来聊聊RRT家族里的informed - RRT*算法，同时看看与之相关的其他RRT系列算法，还会穿插一些MATLAB代码示例哦。

RRT家族成员初览

RRT家族里有多个成员，像RRT、RRT - Connect（双向RRT ）、RRT以及我们重点要说的informed - RRT 。每个成员都有其独特的优势和适用场景。

RRT算法

RRT算法的核心思想比较简单直观，它通过随机采样在搜索空间中逐步构建一棵搜索树。下面是一段简单的RRT MATLAB代码示例（这里只是示意关键部分，并非完整可运行代码）：

% 初始化树，根节点为起始点
tree = [start_point]; 
while ~goal_reached
    % 随机采样一个点
    sample = sample_free_space(); 
    % 在树中找到距离采样点最近的节点
    nearest = nearest_neighbor(tree, sample); 
    % 从最近节点向采样点扩展一段距离
    new_point = steer(nearest, sample); 
    if collision_free(new_point)
        % 将新点添加到树中
        tree = [tree; new_point]; 
        if near_goal(new_point)
            goal_reached = true;
        end
    end
end

这段代码中，samplefreespace函数负责在自由空间中随机采样，nearestneighbor找到树中离采样点最近的节点，steer从最近节点向采样点扩展，collisionfree判断新点是否在障碍物区域，near_goal判断是否接近目标点。RRT算法能快速探索空间，但找到的路径不一定是最优的。

RRT - Connect（双向RRT ）

双向RRT从起始点和目标点同时开始构建搜索树，这大大加快了搜索速度。代码实现上和RRT类似，但需要维护两棵树。

% 初始化起始树和目标树
start_tree = [start_point]; 
goal_tree = [goal_point]; 
while ~trees_connected
    % 随机采样一个点
    sample = sample_free_space(); 
    % 在起始树中找到距离采样点最近的节点
    nearest_start = nearest_neighbor(start_tree, sample); 
    % 在目标树中找到距离采样点最近的节点
    nearest_goal = nearest_neighbor(goal_tree, sample); 
    % 从起始树的最近节点向采样点扩展一段距离
    new_start_point = steer(nearest_start, sample); 
    % 从目标树的最近节点向采样点扩展一段距离
    new_goal_point = steer(nearest_goal, sample); 
    if collision_free(new_start_point) && collision_free(new_goal_point)
        % 将新点添加到相应树中
        start_tree = [start_tree; new_start_point]; 
        goal_tree = [goal_tree; new_goal_point]; 
        if trees_connected(start_tree, goal_tree)
            trees_connected = true;
        end
    end
end

这里多了一棵从目标点开始构建的树，通过同时扩展两棵树，增加了相遇的概率，从而更快找到路径。

RRT*算法

RRT*在RRT的基础上增加了重布线（rewiring）机制，它能在构建树的过程中不断优化路径，逐渐找到更优解。

% 初始化树，根节点为起始点
tree = [start_point]; 
while ~goal_reached
    % 随机采样一个点
    sample = sample_free_space(); 
    % 在树中找到距离采样点最近的节点
    nearest = nearest_neighbor(tree, sample); 
    % 从最近节点向采样点扩展一段距离
    new_point = steer(nearest, sample); 
    if collision_free(new_point)
        % 寻找新点附近的节点集合
        nearby = nearby_nodes(tree, new_point); 
        % 从附近节点集合中找到到新点代价最小的节点
        best_nearby = best_parent(nearby, new_point); 
        % 将新点连接到代价最小的节点
        tree = [tree; new_point]; 
        % 重布线
        for i = 1:size(nearby, 1)
            if cost(nearby(i, :), new_point) < cost(nearby(i, :), nearest)
                % 重新连接节点
                tree = rewire(tree, nearby(i, :), new_point); 
            end
        end
        if near_goal(new_point)
            goal_reached = true;
        end
    end
end

这段代码中，nearbynodes找到新点附近的节点，bestparent从附近节点中找代价最小的节点来连接新点，rewire进行重布线操作，不断优化树的结构，使路径更优。

informed - RRT*算法

informed - RRT 是在RRT基础上进一步优化的算法，它利用目标信息来引导搜索，从而更快地找到全局最优解。它通过在目标点周围定义一个椭圆区域，在这个区域内进行更密集的采样，提高找到最优路径的效率。

% 初始化树，根节点为起始点
tree = [start_point]; 
% 计算起始点到目标点的距离
distance_to_goal = norm(start_point - goal_point); 
% 根据距离计算椭圆参数
a = distance_to_goal; 
b = distance_to_goal; 
while ~goal_reached
    % 在椭圆区域内采样
    sample = sample_ellipse(a, b, goal_point); 
    % 在树中找到距离采样点最近的节点
    nearest = nearest_neighbor(tree, sample); 
    % 从最近节点向采样点扩展一段距离
    new_point = steer(nearest, sample); 
    if collision_free(new_point)
        % 寻找新点附近的节点集合
        nearby = nearby_nodes(tree, new_point); 
        % 从附近节点集合中找到到新点代价最小的节点
        best_nearby = best_parent(nearby, new_point); 
        % 将新点连接到代价最小的节点
        tree = [tree; new_point]; 
        % 重布线
        for i = 1:size(nearby, 1)
            if cost(nearby(i, :), new_point) < cost(nearby(i, :), nearest)
                % 重新连接节点
                tree = rewire(tree, nearby(i, :), new_point); 
            end
        end
        if near_goal(new_point)
            goal_reached = true;
        end
    end
end

这里的sample_ellipse函数在定义的椭圆区域内采样，相比RRT*随机采样，它能更有针对性地向目标区域搜索，大大提高了找到全局最优路径的速度。

MATLAB编码优势

使用MATLAB进行这些算法的编码有不少好处。比如说，我们可以快速自定义不同维度的栅格地图。在代码编写上采用模块化方式，并且遵循编程规范，这对于算法进一步扩展和创新非常有帮助，而且便于和其他算法联合仿真。例如，我们可以把地图生成、碰撞检测、路径搜索等功能封装成不同的函数模块，这样无论是修改算法细节还是与其他算法结合，都变得更加容易。

总之，RRT家族的这些算法各有千秋，informed - RRT*凭借其独特的搜索策略在寻找全局最优轨迹规划上有着出色的表现，而MATLAB为我们实现和研究这些算法提供了一个便捷的平台。希望今天的分享能让大家对这些算法有更深入的了解。