【路径规划】一种考虑拥塞的改进路径规划算法[CCPF-RRT*]（Matlab代码实现）-CSDN博客

本文链接：https://blog.csdn.net/weixin_67304359/article/details/148427252

👨‍🎓个人主页

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

一、背景介绍

二、CCPF-RRT*算法概述

三、算法核心步骤

四、优势与局限性

五、总结与展望

📚2 运行结果

2.1 RRTStar

2.2 PFRRTStar

2.3 PQRRTStar

2.4 CCPF_RRTStar

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

RRT 算法是以起始点为随机树的根节点，通过在状态空间中以增量采样方式获取随机采样点，并使树向着采样点位置扩展生长出与危险区域无碰撞的树节点，直到达到目标点 [2]。RRT 树扩展过程如图 1 所示。

RRT 算法具有概率完备性，当节点数量趋于无穷大时，找到存在的可行路径的概率无限接近1[3]。但由于路径搜索是通过在空间中随机采样进行的，规划出的路径随机性大，且不能保证所得路径是最优路径或次最优路径。当空间较大、障碍物较多时，运算量增加，导致搜索时间较长 [4]。

【路径规划】一种考虑拥塞的改进路径规划算法[CCPF-RRT*]研究

一、背景介绍

路径规划是机器人技术、自动驾驶、工业生产等领域中的基础问题，其核心在于如何在复杂环境中找到一条从起始点到目标点的最优或次优路径。然而，传统路径规划算法在处理大规模场景或存在动态障碍物（如交通拥塞）时，往往存在搜索效率低、路径优化差等问题。为了克服这些不足，研究人员提出了多种改进的RRT（Rapidly-exploring Random Trees，快速扩展随机树）算法，其中CCPF-RRT（考虑拥塞的改进RRT算法）便是一种针对拥塞环境的创新算法。

二、CCPF-RRT*算法概述

CCPF-RRT算法在RRT算法的基础上，引入了拥塞成本评估机制，使得算法在规划路径时能够考虑到路径上的拥塞情况，从而生成更加合理、高效的路径。具体来说，该算法在构建随机树的过程中，不仅考虑了节点间的几何距离，还考虑了节点间的拥塞成本，通过启发式代价函数来综合评估路径的优劣。

三、算法核心步骤

初始化：设置起始点、目标点以及拥塞成本评估的初始参数。
随机采样：在搜索空间中随机采样生成新的候选节点。
拥塞成本评估：对于每个候选节点，根据其周围的拥塞情况（如交通流量、障碍物密度等）评估其拥塞成本。
节点选择与扩展：综合考虑候选节点的几何距离和拥塞成本，选择最优的节点进行扩展，并更新随机树。
路径优化：在随机树生长过程中，不断对已生成的路径进行优化，以减小总成本（包括几何距离和拥塞成本）。
终止条件：当随机树扩展到目标点附近或达到预设的迭代次数时，算法终止，并输出最优或次优路径。

四、优势与局限性

优势：

考虑拥塞：CCPF-RRT*算法在规划路径时能够考虑到拥塞情况，使得生成的路径更加符合实际交通状况。
优化效率高：通过启发式代价函数和路径优化策略，算法能够生成更加高效、合理的路径。
适应性强：算法适用于各种复杂环境，包括大规模场景和动态障碍物环境。

局限性：

计算复杂度较高：由于引入了拥塞成本评估和路径优化策略，算法的计算复杂度相对较高，可能需要较长的运行时间。
参数敏感：算法的性能受到拥塞成本评估参数的影响，需要仔细调整以获得最佳效果。

五、总结与展望

CCPF-RRT*算法作为一种考虑拥塞的改进路径规划算法，在处理复杂交通环境和动态障碍物方面具有显著优势。然而，其计算复杂度和参数敏感性等问题仍需进一步研究和优化。未来工作可以围绕如何降低计算复杂度、提高算法实时性以及优化参数设置等方面展开，以推动路径规划技术的发展和应用。

📚2 运行结果

2.1 RRTStar

%% 构建颜色MAP图
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);
myColor=[16 , 232, 7  ;
         242, 213, 7  ;
         219, 96 , 4  ;
         242, 7  , 124;
         108, 75 , 235;
         0  , 1  , 1  ;
         1  , 0  , 0  ;];
for i=1:size(myColor)
    myColor(i,:)=myColor(i,:)/norm(myColor(i,:));
end
%% 构建栅格地图场景
% 栅格界面大小:行数和列数
rows = 10;
cols = 10; 

% % 定义栅格地图全域，并初始化空白区域
% field = ones(rows, cols);
% 
% % 障碍物区域
% obsRate = 0.3;
% obsNum = floor(rows*cols*obsRate);
% obsIndex = randi([1,rows*cols],1,obsNum);
% field(obsIndex) = 2;
% 
% start=[1,1];%起点
% goal=[10,10];%终点
% field(start(1),start(2)) = 1;
% field(goal(1),goal(2)) = 1;
% field;
% [oby,obx]=find(field==2);
% obr=ones(size(obx));
% obstacle=[obx,oby,obr];

% start=[1.5,1.5];%起点
% goal=[10.5,10.5];%终点
start=[1,1];%起点
goal=[11,11];%终点

area=[1,rows+1,1,cols+1];%范围
plotArea=area;%范围
tree=[];%树
prob=0.1;%搜索概率
stepSize=0.5;%步长
DISCRETE=0.05;%检测障碍的离散步长
% obstacleNum=10;%障碍数量
% obstaclePoint=randi(8,obstacleNum,2)+2;%障碍坐标
% obstacleR=randi(10,obstacleNum,1)./10;%障碍大小
% obstacle=[obstaclePoint obstacleR];%障碍
obstacle=[3,3,1.3;3,7,1.3;8,9,1.5;7.7,3,1.6;];%障碍
% obstacle=[5,2,3;7,10,3;];%障碍
nearByR=5;%newNode搜索范围
iter=0;%迭代次数

2.2 PFRRTStar

2.3 PQRRTStar

2.4 CCPF_RRTStar

部分代码：

%% 构建颜色MAP图
cmap = [1 1 1; ... % 1-白色-空地
0 0 0; ... % 2-黑色-静态障碍
1 0 0; ... % 3-红色-动态障碍
1 1 0;... % 4-黄色-起始点
1 0 1;... % 5-品红-目标点
0 1 0; ... % 6-绿色-到目标点的规划路径
0 1 1]; % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);
myColor=[16 , 232, 7 ;
242, 213, 7 ;
219, 96 , 4 ;
242, 7 , 124;
108, 75 , 235;
0 , 1 , 1 ;
1 , 0 , 0 ;];
for i=1:size(myColor)
myColor(i,:)=myColor(i,:)/norm(myColor(i,:));
end
%% 构建栅格地图场景
% 栅格界面大小:行数和列数
rows = 10;
cols = 10;

% % 定义栅格地图全域，并初始化空白区域
% field = ones(rows, cols);
%
% % 障碍物区域
% obsRate = 0.3;
% obsNum = floor(rows*cols*obsRate);
% obsIndex = randi([1,rows*cols],1,obsNum);
% field(obsIndex) = 2;
%
% start=[1,1];%起点
% goal=[10,10];%终点
% field(start(1),start(2)) = 1;
% field(goal(1),goal(2)) = 1;
% field;
% [oby,obx]=find(field==2);
% obr=ones(size(obx));
% obstacle=[obx,oby,obr];

% start=[1.5,1.5];%起点
% goal=[10.5,10.5];%终点
start=[1,1];%起点
goal=[11,11];%终点

area=[1,rows+1,1,cols+1];%范围
plotArea=area;%范围
tree=[];%树
prob=0.1;%搜索概率
stepSize=0.5;%步长
DISCRETE=0.05;%检测障碍的离散步长
% obstacleNum=10;%障碍数量
% obstaclePoint=randi(8,obstacleNum,2)+2;%障碍坐标
% obstacleR=randi(10,obstacleNum,1)./10;%障碍大小
% obstacle=[obstaclePoint obstacleR];%障碍
obstacle=[3,3,1.3;3,7,1.3;8,9,1.5;7.7,3,1.6;];%障碍
% obstacle=[5,2,3;7,10,3;];%障碍
nearByR=5;%newNode搜索范围
iter=0;%迭代次数