RRT-connect路径B样条平滑后碰撞迭代优化陷入死循环求助

我通过RRT-connect算法生成初始路径，经Ramer–Douglas–Peucker算法剪枝减少路点后，使用MATLAB内置的三次B-spline函数bsplinepolytraj进行平滑处理。随后通过collisionChecking函数检测平滑路径是否无碰撞，当检测到碰撞时，调用reformCollideSegment函数对碰撞路段进行优化，循环执行直至路径无碰撞，但程序陷入无限循环，始终无法生成无碰撞路径，寻求解决思路。

附件文件 #

• 剪枝前的初始路径
• 剪枝后路径文件：pathReduced.mat
• 存在碰撞的平滑路径文件：smoothedPath.mat
• 环境地图文件：environmentMatrix.mat

代码片段 #

%% Path reduction
% reducepoly function Reduce density of points in ROI using Ramer–Douglas–Peucker
% algorithm.
% P_reduced = reducepoly(P,tolerance) reduces the density of points in array
% P, where tolerance specifies how much a point can deviate from a straight
% line.
tolerance = 0.03;
pathReduced = reducepoly(path,tolerance);
% Check the number of row for pathReduced (must be at least 4 rows)
[row, column] = size(pathReduced);
if row < 4
    pathReduced = path;
end

%% Path smoothing sing B-spline smoothing function
% Inputs:
% cpts = pathReduced';
cpts = path';
% Calling Smoothing Funtion
[pathSmooth, pathLengthS, qd, qdd, pp] = BSpline(cpts);

%% Collision detection for the smoothed path and refinment
% Collision detection is performed by discrete samples
[cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
keyboard;
while ~isempty(iwant)
    [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);
    pathReduced = pathReducedRefined;
    cpts = pathReduced';
    % Calling Smoothing Funtion
    [pathSmooth, ComputationTime2, pathLengthS] = BSpline(cpts);
    [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
end

function [cc, iwant, Time, collideIndices] = collisionChecking(map, pathSmooth)
% % FUNCTION COLLISIONCHECKING test for collision occurence.
% %
% % Inputs:
% %   map         - logical map of size 500x500
% %   pathSmooth  - smoothed path in cartesian form produced using
% %                   [pathSmooth, pathLengthS] = BSpline(cpts).
% %
% % Outputs:
% %   cc          - flage of collision occurence: logic 1 represents
% %   collision, while logic 0 represetns collision-free condition
% %

tic;

v = pathSmooth;
env = map;
cc = true;
% Initialize arrays for storing intersections and their indices
iwant = zeros([],2) ;           % To store the coordinates of intersections
collideIndices = zeros([]);   % To store the indices in v that correspond to iwant
count = 0;
% Loop through the path v
for ii = 1:length(v)-1
    % Check if the point in v is in an obstacle-free area (intersection)
    if env(round(v(ii,1)), round(v(ii,2)))  % Use (y,x) indexing for the environment
        cc = false;
        %         disp('There is no intersection');
    else
        count = count + 1;
        
        iwant(count,:) = [round(v(ii,2)), round(v(ii,1))];  % Store the point in iwant
        collideIndices(count) = ii;                         % Store the index from v
        cc = true;
        %         disp('There is intersection');
    end
end

Time = toc;
end

问题排查与解决建议 #

1. 修复平滑函数的输入错误 #

代码中明确注释了cpts = pathReduced'，但实际写的是cpts = path'，导致平滑处理的是原始路径而非剪枝后的路径，后续的碰撞优化完全没有作用在正确的路径控制点上，这是循环无法收敛的核心原因之一。修改为：

cpts = pathReduced';

2. 修正碰撞检测逻辑的两个关键错误 #

• 坐标索引顺序混乱：MATLAB矩阵采用(行,列)即(y,x)索引，但代码中同时混用env(round(v(ii,1)), round(v(ii,2)))和iwant(count,:) = [round(v(ii,2)), round(v(ii,1))]，容易导致碰撞点判断错误。统一为路径点(x,y)对应地图索引(y,x)：

  x = round(v(ii,1));
  y = round(v(ii,2));
  % 先判断是否越界（越界视为碰撞）
  if x < 1 || x > size(env,2) || y < 1 || y > size(env,1)
      count = count + 1;
      iwant(count,:) = [x, y];
      collideIndices(count) = ii;
      cc = true;
  elseif ~env(y, x)  % 假设env(y,x)=1表示自由空间，0表示障碍物
      count = count + 1;
      iwant(count,:) = [x, y];
      collideIndices(count) = ii;
      cc = true;
  else
      cc = false;
  end
  

• 漏掉最后一个路径点检测：循环for ii = 1:length(v)-1会跳过最后一个点，修改为for ii = 1:length(v)。

3. 约束reformCollideSegment的优化逻辑 #

如果reformCollideSegment仅在碰撞路段插值新增节点，却未确保新增节点位于自由空间，平滑后的路径依然可能穿障。建议：

• 强制新增的控制点必须落在地图的自由区域内；
• 若无法确保，改用带碰撞约束的B样条平滑（比如结合fmincon做带约束的路径优化，限制样条曲线全程不进入障碍物区域）。

4. 增加循环迭代次数限制 #

避免因逻辑错误导致无限循环，给循环添加最大迭代次数：

maxIter = 20;
iter = 0;
while ~isempty(iwant) && iter < maxIter
    iter = iter + 1;
    [pathReducedRefined] = reformCollideSegment(iwant, pathReduced);
    pathReduced = pathReducedRefined;
    cpts = pathReduced';
    [pathSmooth, ComputationTime2, pathLengthS] = BSpline(cpts);
    [cc, iwant, computationTime3] = collisionChecking(map, pathSmooth);
    fprintf('当前迭代次数：%d\n', iter);
end
if iter == maxIter
    warning('已达最大迭代次数，仍未生成无碰撞路径');
end

5. 调整剪枝算法的容差参数 #

当前tolerance=0.03可能过小（导致剪枝后路径点过多）或过大（导致剪枝路径过于贴近障碍物，平滑后容易穿障）。建议：

• 逐步调整tolerance值，观察剪枝后路径的安全性；
• 在剪枝阶段加入直线段碰撞检测，确保剪枝后的任意两个相邻路径点之间的直线无碰撞，为后续平滑提供安全的基础路径。

内容的提问来源于stack exchange，提问作者AboRiam