✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

 👇 关注我领取海量matlab电子书和数学建模资料 

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍 

一、背景

（一）轨迹规划在机器人领域的重要性

在机器人技术的广泛应用中，轨迹规划是核心任务之一。无论是工业生产线上的机械臂，还是服务机器人在复杂室内环境中的自主导航，亦或是无人驾驶车辆在道路上的行驶，机器人都需要规划出一条安全、高效的轨迹，以完成指定任务。精确的轨迹规划不仅能提高机器人的工作效率，还能确保其运行的安全性，避免与周围环境中的障碍物发生碰撞。

（二）复杂环境带来的挑战

现实环境往往具有高度复杂性，充满了各种形状、位置和动态变化的障碍物。例如，在城市街道中，无人驾驶车辆需要避开其他车辆、行人、交通设施等；在室内环境中，服务机器人可能会遇到家具、人员走动等障碍物。此外，环境信息的获取也存在一定难度，传感器的精度、检测范围以及噪声干扰等因素，都会影响机器人对环境的准确感知。因此，如何在复杂且不确定的环境中快速、有效地规划出可行轨迹，是轨迹规划面临的关键挑战。

（三）RRT 算法与 pOGMs 结合的意义

1. RRT 算法优势

   ：快速探索随机树（RRT）算法是一种常用于解决高维空间中路径规划问题的搜索算法。它具有良好的扩展性和对复杂环境的适应性，能够在复杂的构型空间中快速搜索出一条可行路径。RRT 算法通过随机采样点来扩展搜索树，不需要对整个空间进行详尽的搜索，因此在处理大规模、复杂环境时效率较高。

2. pOGMs 优势

   ：概率占用格地图（pOGMs）是一种对环境进行建模的有效方法。它将环境空间划分为多个网格单元，每个网格单元都有一个概率值，表示该单元被障碍物占用的可能性。pOGMs 能够很好地处理环境中的不确定性，通过融合多个传感器的信息，提高环境建模的准确性。此外，pOGMs 可以方便地更新，以适应动态变化的环境。

3. 两者结合的优势

   ：将 RRT 算法与 pOGMs 相结合，可以充分发挥两者的优势。pOGMs 为 RRT 算法提供准确的环境模型，使得 RRT 算法在搜索路径时能够更好地避开障碍物；而 RRT 算法则可以在 pOGMs 构建的环境模型上快速搜索出可行轨迹，提高轨迹规划的效率。这种结合方式能够更有效地应对复杂、不确定的环境，为机器人的轨迹规划提供更可靠的解决方案。

二、原理

（一）快速探索随机树（RRT）算法原理

1. 基本概念

   ：RRT 算法通过构建一棵搜索树来寻找从起始点到目标点的路径。树中的每个节点代表机器人的一个构型（如位置和姿态），节点之间的边表示机器人从一个构型到另一个构型的运动。

2. 算法流程

   ：

   • 初始化

     ：首先初始化搜索树，将起始点作为树的根节点。

   • 随机采样

     ：在构型空间中随机采样一个点 qrand。这个点的生成是随机的，但可以通过一些策略（如偏向目标点采样）来提高搜索效率。

   • 寻找最近节点

     ：在搜索树中找到距离 qrand 最近的节点 qnear。通常通过计算采样点与树中所有节点的距离（如欧几里得距离）来确定最近节点。

   • 扩展树

     ：从 qnear 向 qrand 扩展一个新的节点 qnew。扩展的方式是沿着从 qnear 到 qrand 的方向，按照一定的步长移动。在扩展过程中，需要检查新节点是否与障碍物冲突（这就需要借助 pOGMs 提供的环境信息）。如果新节点不与障碍物冲突，则将其添加到搜索树中，并建立与 qnear 的连接。

   • 判断目标

     ：检查新添加的节点 qnew 是否接近目标点。如果接近目标点，则认为找到了一条可行路径，可以通过回溯搜索树得到从起始点到目标点的完整路径；否则，重复上述随机采样、寻找最近节点、扩展树的步骤，直到找到目标点或达到预设的搜索次数。

（二）概率占用格地图（pOGMs）原理

1. 环境离散化

   ：pOGMs 将连续的环境空间划分为一个个小的网格单元。每个网格单元都具有相同的大小，这样可以将复杂的环境空间简化为离散的网格表示，便于进行计算和处理。

2. 概率估计

   ：对于每个网格单元，通过传感器信息或其他方式来估计其被障碍物占用的概率。例如，在机器人配备激光雷达的情况下，激光雷达发射的激光束遇到障碍物会反射回来，根据反射信号可以确定障碍物的位置。通过对多个激光束数据的处理，可以计算出每个网格单元被障碍物占用的概率。通常，使用贝叶斯方法来更新网格单元的概率值。假设 p(mi) 是网格单元 i 被占用的先验概率，z 是传感器测量值，根据贝叶斯公式 p(mi∣z)=p(z)p(z∣mi)p(mi) 来更新概率，其中 p(z∣mi) 是在网格单元 i 被占用的情况下得到测量值 z 的概率，p(z) 是测量值 z 的概率。

3. 不确定性处理

   ：pOGMs 能够很好地处理环境中的不确定性。由于传感器测量存在误差，环境中的障碍物位置也可能存在不确定性，pOGMs 通过概率值来表示这种不确定性。例如，一个网格单元的概率值为 0.5，表示该单元有 50% 的可能性被障碍物占用，这种表示方式更符合实际环境的不确定性情况。同时，通过不断融合新的传感器信息，可以逐渐提高概率估计的准确性，从而更准确地描述环境。

（三）基于 RRT 算法和 pOGMs 的轨迹规划原理

1. 环境建模与信息交互

   ：首先利用 pOGMs 对环境进行建模，将环境信息转化为网格单元的概率表示。RRT 算法在进行路径搜索时，通过查询 pOGMs 中网格单元的概率值来判断新节点是否与障碍物冲突。如果某个网格单元的概率值超过一定阈值（如 0.5），则认为该单元所在位置存在障碍物，新节点不能扩展到该位置。这样，pOGMs 为 RRT 算法提供了准确的环境信息，指导 RRT 算法在搜索路径时避开障碍物。

2. 动态环境适应

   ：在动态环境中，障碍物的位置可能会发生变化。pOGMs 可以根据新的传感器信息实时更新网格单元的概率值，从而反映环境的动态变化。RRT 算法在搜索过程中，不断查询更新后的 pOGMs，及时调整搜索方向，以适应环境的变化。例如，当检测到一个新的障碍物出现时，pOGMs 会更新相应网格单元的概率值，RRT 算法在后续的扩展过程中就会避开该区域，重新规划可行路径。

3. 轨迹优化

   ：在通过 RRT 算法找到一条可行路径后，可以对路径进行优化。由于 RRT 算法生成的路径可能不是最优的，存在一些不必要的曲折或过长的线段。可以采用一些优化方法，如路径平滑算法，对路径进行处理，使其更加平滑、高效。例如，通过去除路径中的一些冗余节点，或者对路径进行插值处理，使机器人的运动更加流畅，提高轨迹规划的质量。

⛳️ 运行结果

📣 部分代码

% Function - Collision check along the edge--------------------------------

function [col, p_step, p_pos_step, p_angle, p_vel, p_prob] = InCollision_Edge(p_pos, ~, l_angle, p_angle, l_vel, angle_diff, gridConverted, EGO, param, sampleTime)

% Determining the change in the steering angle

if (p_angle >=0 && p_angle <=90 && l_angle >= 270 && l_angle <=360)

    if abs(angle_diff) > EGO.steerMax

        p_angle = l_angle+EGO.steerMax;

    end

elseif (l_angle >=0 && l_angle <=90 && p_angle >= 270 && p_angle <=360)

    if abs(angle_diff) > EGO.steerMax

        p_angle = l_angle-EGO.steerMax;

    end

else

    if abs(angle_diff) > EGO.steerMax && angle_diff < 0

        p_angle = l_angle+EGO.steerMax;

    elseif abs(angle_diff) > EGO.steerMax && angle_diff > 0

        p_angle = l_angle-EGO.steerMax;

    end

end

if p_angle < 0

    p_angle = 360+p_angle;

end

% Initialise variables

p_step = zeros(1,2);

p_pos_step = zeros(1,2);

P = zeros(8,1);

P_grid = zeros(8,1);

probOccupancy = zeros(5,1);

col = 0;

% Estimation of distance for every step of RRT

minRes = l_vel*param.simulationSampleTime + 0.5*EGO.ax*param.simulationSampleTime^2; % steps in between RRT

minDist = l_vel*param.RRTSampleTime + 0.5*EGO.ax*param.RRTSampleTime^2; % distance constrained based on RRT sample time

p_vel = l_vel+EGO.ax*param.RRTSampleTime; 

% To prevent negative velocities

if p_vel <=0 

    p_vel = 0;

end

m = ceil(minDist/minRes);

s = linspace(0, minDist, m);

ex=cosd(p_angle);

ey=sind(p_angle);

t_x = s*ex;

t_y = s*ey;

exOrtho=ex*cos(pi/2)+ey*sin(pi/2);

eyOrtho=-ex*sin(pi/2)+ey*cos(pi/2);

for i = 2:m

    p_pos_step(1) = p_pos(1)+t_x(i);

    p_pos_step(2) = p_pos(2)+t_y(i);

    P(1)=p_pos_step(1)+ex*(EGO.lf)+EGO.w/2*exOrtho;

    P(2)=p_pos_step(2)+ey*(EGO.lf)+EGO.w/2*eyOrtho;

    P(3)=p_pos_step(1)+ex*(EGO.lf)-EGO.w/2*exOrtho;

    P(4)=p_pos_step(2)+ey*(EGO.lf)-EGO.w/2*eyOrtho;

    P(5)=p_pos_step(1)-ex*(EGO.lr)+EGO.w/2*exOrtho;

    P(6)=p_pos_step(2)-ey*(EGO.lr)+EGO.w/2*eyOrtho;

    P(7)=p_pos_step(1)-ex*(EGO.lr)-EGO.w/2*exOrtho;

    P(8)=p_pos_step(2)-ey*(EGO.lr)-EGO.w/2*eyOrtho;

    % Conversion to grid

    p_step(1) = round(p_pos_step(1)/0.5);

    p_step(2) = round(p_pos_step(2)/0.5);

    P_grid = round(P/(0.5));

    % Checking for occupancy value

    probOccupancy(1) = gridConverted(p_step(1), p_step(2), sampleTime);

    probOccupancy(2) = gridConverted(P_grid(1), P_grid(2), sampleTime);

    probOccupancy(3) = gridConverted(P_grid(3), P_grid(4), sampleTime);

    probOccupancy(4) = gridConverted(P_grid(5), P_grid(6), sampleTime);

    probOccupancy(5) = gridConverted(P_grid(7), P_grid(8), sampleTime);  

    if any(probOccupancy > 0)

        p_prob = max(probOccupancy);

    if any(probOccupancy >= param.minThresh)

        col = 1;

        return;

    end

    else

        p_prob = 0;

    end

end

end

🔗 参考文献

🍅往期回顾扫扫下方二维码