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

👨‍🎓个人主页：研学社的博客    

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

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

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

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

目录

💥1 概述

📚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]。

📚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;%迭代次数

🎉3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]Yanming Liang (2023) CCPF-RRT*: An improved path planning algorithm with consideration of congestion

[2]郭茜,袁德成.基于改进RRT*算法的可重构机器人路径规划[J].工业仪表与自动化装置,2023(03):70-74.DOI:10.19950/j.cnki.cn61-1121/th.2023.03.014.

[3]黄恒一.改进RRT机器人路径规划技术研究[J].物联网技术,2023,13(05):92-94.DOI:10.16667/j.issn.2095-1302.2023.05.025.

🌈4 Matlab代码实现