while failedAttempts <= maxFailedAttempts % 循环生成rrt树 %% 选择一个随机配置 if rand < 0.5 sample = rand(1,6) .* searchSize-180; % 随机样本 else sample = S_goal; % 以样本为目标生成偏置树 end %% 选择RRT树中最接近qrand的节点 [A, I] = min( distanceCost(RRTree(:,1:6),sample) ,[],1); % 找出离采样点最近的节点 closestNode = RRTree(I(1),1:6); %% 从qnearest向qrand方向移动一个增量距离 movingVec = sample-closestNode; movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化 newPoint = closestNode + stepsize * movingVec; if ~StaticCollisionDetection_sphere(newPoint, circleCenter,r,choice_L) % 如果树中最近的节点延伸到新点是可行的 failedAttempts = failedAttempts + 1; continue; end %将符合位置要求的节点加入树中： if distanceCost(newPoint,S_goal) < threshold pathFound = true; break; end % 达到目标 [A, I2] = min( distanceCost(RRTree(:,1:6),newPoint) ,[],1); % 检查新节点是否已经存在于树中 if distanceCost(newPoint,RRTree(I2(1),1:6)) < threshold, failedAttempts = failedAttempts + 1; continue; end RRTree = [RRTree; newPoint I(1)]; % 添加节点 if length(RRTree(:,1))>1500 RRTree = double([S_start -1]); search=search+1; if display close; %回溯规划轨迹，从父信息中检索路径：

该代码段是一个RRT（Rapidly-exploring Random Tree）路径规划算法的实现。该算法通过随机采样的方式不断生成一棵树，直到找到一条连接起点和终点的路径。具体实现中，算法会随机生成一个样本点，然后在已有的树中找到距离样本点最近的节点，从该节点出发，朝着样本点方向移动一定距离，得到一个新的节点。如果新节点与其他障碍物没有碰撞，则将该节点添加到树中。如果新节点距离终点足够近，则认为找到了一条路径。如果没有找到路径，则继续随机生成样本点，直到达到最大失败次数或者找到一条路径为止。

该代码段中的具体实现涉及到一些变量和函数。其中，distanceCost函数用于计算两个点之间的距离；StaticCollisionDetection_sphere函数用于检测新节点是否与其他障碍物发生碰撞；searchSize表示采样空间的大小；stepsize表示每次移动的距离；threshold表示到达终点的距离阈值；maxFailedAttempts表示最大失败次数；RRTree表示已有的树，每个节点包含位置信息和父节点的索引；S_goal和S_start分别表示终点和起点的位置信息；circleCenter和r表示障碍物的圆心位置和半径大小。

相关问题

while failedAttempts <= maxFailedAttempts % 循环来增长rrt loop to grow RRTs %% 选择一个随机配置 chooses a random configuration if rand < 0.5 sample = rand(1,2) .* size(map); % 随机样本 random sample else sample = goal; % 以样本为目标，使树生成偏向目标 sample taken as goal to bias tree generation to goal end

这段代码是RRT算法中的循环，用于不断扩展树结构，直到达到最大失败次数或者找到可行路径为止。具体过程如下：

- 首先判断失败次数是否超过了最大失败次数，如果是则停止扩展，返回空路径或者最优路径。
- 对于每个循环，从随机样本空间中选择一个随机样本，作为要添加到树中的节点。
- 以50%的概率选择随机样本或者目标点作为要添加的节点。这个操作可以使树的生长偏向目标点，加速搜索过程。
- 如果选择的是随机样本，则将其坐标随机生成，生成的坐标范围由地图的大小确定。
- 如果选择的是目标点，则将目标点作为要添加的节点。
- 这个过程会不断重复，直到达到最大失败次数或者找到可行路径为止。

这个循环是RRT算法的核心部分，它不断地生成新的节点，将其添加到树中，并且连接到树中最近的节点。通过这个过程，可以逐步构建出一棵树，表示搜索空间中的可行路径。

Informed-rrt椭球区域随机采样策略

### Informed-RRT* 椭球区域内随机采样策略

Informed-RRT* 是一种改进版本的 RRT* 算法，其核心在于通过限制采样空间来加速最优路径的发现过程。具体来说，在找到一条可行路径之后，算法会动态调整一个椭球形区域内的采样范围，使得后续样本更加集中在可能改善当前最佳路径的位置。

#### 椭球定义与更新机制

当首次获得连接起始节点至目标节点的有效路径时，计算此路径的成本 \( c_{\text{best}} \)，并以此为基础构建初始椭圆体。随着迭代次数增多以及更优路径不断涌现，\( c_{\text{best}} \) 值逐渐减小，相应地缩小了椭圆形边界尺寸，从而聚焦于更有潜力提升整体性能的方向上[^2]。

#### 随机采样流程

为了确保每次都能高效选取位于上述约束条件下的候选位置点，采取如下步骤：

1. 计算当前已知最短路径长度；
2. 使用该距离作为半径参数建立围绕起点和终点两个焦点构成的三维椭球面方程；
3. 对满足特定几何关系式的内部体积执行均匀分布抽样操作；

这种方法有效减少了不必要的探索活动，显著提升了搜索效率的同时也保证了理论上的全局收敛特性[^4]。

function sample = informedSample(c_best, start_pos, goal_pos)
    % 定义椭球中心坐标及轴向尺度因子
    center = (start_pos + goal_pos)/2;
    
    a = norm(goal_pos - start_pos);  % 半长轴等于两定点间欧氏间距的一半
    b = sqrt((c_best^2 / 4) - ((a/2)^2));  % 根据勾股定理求得半短轴
    
    while true
        phi = rand * pi; theta = acos(rand*(2)-1);
        
        x_prime = cos(phi)*sin(theta);
        y_prime = sin(phi)*sin(theta);
        z_prime = cos(theta);

        scale_factor = min([b/a, 1]);
        scaled_point = [x_prime,y_prime,z_prime]*scale_factor*a;

        candidate = center + scaled_point';
        
        if isInsideEllipsoid(candidate,center,a,b)
            break;
        end
    end
    
    sample = candidate;
end

% 判断给定点是否处于指定椭球体内
function flag = isInsideEllipsoid(point,center,a,b)
    vec = point-center;
    dist_sqrd = dot(vec./[a;a],vec./[a;a]);

    flag = (dist_sqrd <= 1);
end