【无人机控制】基于RRT算法实现无人机系统在充满障碍物的环境避碰控制附matlab代码

简介: 【无人机控制】基于RRT算法实现无人机系统在充满障碍物的环境避碰控制附matlab代码

✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信。

🍎个人主页:Matlab科研工作室

🍊个人信条:格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信      无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机

⛄ 内容介绍

  经典的路径规划算法大都需要在全局已知空间中对环境进行建模,包括人工势场法,遗传算法,启发式算法,仿生学算法等.由于需要预先构建环境,因此这些方法并不适合解决在高维度空间中的路径规划问题.基于快速扩展随机树(RRT)的路径规划方式其优势在于可以避免对全局环境的构建,通过对状态空间进行随机采样,检测碰撞点,能够有效地解决在平面及三维状态空间下的复杂路径规划问题.通过与人工势场法和A*算法进行比对,确定了RRT算法在复杂环境中解决无人机路径规划问题的优势,在对相关参数进行优化后该方法是概率完备且存在最优解的,同时在固定翼智能集群飞行编队控制及协同项目中应用.

⛄ 部分代码

clearvars

close all

rand('state',1);

x_max = 500;

y_max = 500;

%obstacle = [500,150,200,200];

obstacle1 = [0,200,100,100];

obstacle2 = [200,200,200,100];

obstacle3 = [500,0,100,500];

obstacle4 = [0,500,500,100];

ob_vec=[obstacle1;obstacle2;obstacle3;obstacle4];

EPS = 10;

rho=80;

step=10; %which is EPS in old edition

   BETA=EPS/rho; %my addition

%numNodes = 3000;        

numNodes=2000;


q_start.coord = [10 10];

q_start.dir=pi/3; %My addition

q_start.cost = 0;

q_start.parent = 0;

q_goal.coord = [490 490];

q_goal.cost = 0;

q_goal.dir=0;


nodes(1) = q_start;

figure(1)

set(gcf,'unit','normalized','position',[0.4 0.4 0.5 0.5])

axis equal

axis([0 x_max 0 y_max])

%axis([100 110 100 110])


rectangle('Position',obstacle1,'FaceColor',[0 .5 .5])

hold on

rectangle('Position',obstacle2,'FaceColor',[0 .5 .5])

plot(q_goal.coord(1),q_goal.coord(2),'ro','LineWidth',2)




for i = 1:1:numNodes

   

   i

     

   % Break if goal node is already reached

%     IsReached=0;

%         %     for j = 1:1:length(nodes)

%         %         if dist(nodes(j).coord, q_goal.coord)<5

%         %             IsReached=1;

%         %             break

%         %         end

%         %     end

%     if IsReached

%         break;

%     end

   

   

   if rand(1)<0.3

%         disp('sampling: goal 1')

       q_rand=q_goal;

   else

%         disp('sampling: goal 0')

       q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];

       q_rand.dir=(rand(1)-0.5)*2*pi;

   end


   % Pick the closest node from existing list to branch out from

   ndist = [];

   for j = 1:1:length(nodes)

       n = nodes(j);

       %new func

       tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);

       ndist = [ndist tmp];

   end

   [val, idx] = min(ndist);

   q_near = nodes(idx);

   

   

   q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);

   q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);


   

   %/* RG-RRT part: the q_goal is closer to q_near or q_new?

   % while(dist(q_rand.coord,q_near.coord)<dist(q_rand.coord,q_new.coord)) || ~noCollision(q_rand.coord, q_near.coord, obstacle1) || ~noCollision(q_new.coord, q_near.coord, obstacle1) || ~noCollision(q_rand.coord, q_near.coord, obstacle2) || ~noCollision(q_new.coord, q_near.coord, obstacle2)

       

   %/* not RG-RRT    

   while ~noCollision_obs(q_rand.coord, q_near.coord, ob_vec) || ~noCollision_obs(q_new.coord, q_near.coord, ob_vec)  

%         plot(q_rand.coord(1), q_rand.coord(2), 'o', 'Color',  [1 0 0.7410])

       q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];

       q_rand.dir=(rand(1)-0.5)*2*pi;


       % Pick the closest node from existing list to branch out from

       ndist = [];

       for j = 1:1:length(nodes)

           n = nodes(j);

           %jan 28 tmp = dist_Dubin(n, q_rand, BETA, EPS);

           tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);

           ndist = [ndist tmp];

       end

       [val, idx] = min(ndist);

       q_near = nodes(idx);

       

       q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);

       q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);

   end

%     plot(q_rand.coord(1), q_rand.coord(2), 'x', 'Color',  [0 0.4470 0.7410])

   %*/

   

   % rewiring process

   if noCollision_obs(q_rand.coord, q_near.coord, ob_vec) && noCollision_obs(q_new.coord, q_near.coord, ob_vec)

%         line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], 'Color', 'k', 'LineWidth', 2);

%         drawnow

       hold on

       q_new.cost = distDubinThree(q_near, q_new, rho) + q_near.cost;

       

       % Within a radius of r, find all existing nodes

       q_nearest = [];

       r = 100;

       neighbor_count = 1;

       

       % Initialize cost to currently known value

       q_min = q_near;

       C_min = q_new.cost;

       

       for j=1:length(nodes)

           if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<r

               q_nearest(neighbor_count).coord=nodes(j).coord;

               q_nearest(neighbor_count).dir=nodes(j).dir;

               q_nearest(neighbor_count).cost=nodes(j).cost;

               if nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<C_min-2

                   q_min=nodes(j);

                   C_min=nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec);

               end

               neighbor_count=neighbor_count+1;

           end

       end

               

       

       

       

       % Update parent to least cost-from node % near q_near best one

       for j = 1:1:length(nodes)

          if nodes(j).coord == q_min.coord

              q_new.parent = j;

          end

       end

%         q_new.parent=idx; % old q_near

       q_new.cost=C_min; % add by Zhu Wang

%         line([q_min.coord(1),q_new.coord(1)], [q_min.coord(2),q_new.coord(2)],'Color','g');

       % Append to nodes

       nodes = [nodes q_new];

       

       for j=1:length(nodes)-1

           if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(q_new,nodes(j), rho, ob_vec)<r

               if nodes(j).cost>C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec)

                   nodes(j).parent=length(nodes);

                   nodes(j).cost=C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec);

%                     line([nodes(j).coord(1),q_new.coord(1)], [nodes(j).coord(2),q_new.coord(2)],'Color','y');

               end

           end

       end

       

       

       

   end

   if mod(i,100)==1

       text=['i=',int2str(i-1)];

       title(text)

   end

%     if (mod(i,10)==1)

%         F((i-1)/10+1)=getframe(gcf);

%     end

end


D = [];

for j = 1:1:length(nodes)

   tmpdist = distDubinThree_obs(nodes(j), q_goal, rho,ob_vec);

   D = [D tmpdist];

end


%% Search backwards from goal to start to find the optimal least cost path

path=[q_goal.coord q_goal.dir];

[val, idx] = min(D);


q_final = nodes(idx);

q_goal.parent = idx;

q_end = q_goal;


totaldist=q_final.cost+distDubinThree_obs(q_final,q_goal,rho,ob_vec);

q_goal.cost=totaldist;

nodes = [nodes q_goal];

while q_end.parent ~= 0

   start = q_end.parent;

% %     disp(start)

%     plot(q_end.coord(1), q_end.coord(2),'bv')

%     line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], 'Color', 'r', 'LineWidth', 2);

%     drawnow;

%     hold on;

   q_end = nodes(start);

%     F=[F getframe(gcf)];

   

   path=[q_end.coord q_end.dir; path];

end

% plot(path(:,1),path(:,2),'k--');

⛄ 运行结果

⛄ 参考文献

[1] 王永皎,郑春峰.基于RRT算法的自由飞行空间机器人避碰运动规划[J].农机化研究, 2006(3):3.DOI:10.3969/j.issn.1003-188X.2006.03.017.

[2] 刘强,阳小燕,朱克佳.基于RRT算法的桥检无人机航线规划仿真系统设计[J].计算机科学与应用, 2022(008):012.

[3] 李华忠,梁永生,但唐仁,等.基于RRT的机器人避碰运动规划算法研究[J].深圳信息职业技术学院学报, 2012, 10(3):8.DOI:10.3969/j.issn.1672-6332.2012.03.005.

[4] 马蓉.基于改进RRT算法的无人机航路规划与跟踪方法研究[J].导航定位与授时, 2020, 7(1):6.DOI:CNKI:SUN:DWSS.0.2020-01-004.

⛳️ 代码获取关注我

❤️部分理论引用网络文献,若有侵权联系博主删除
❤️ 关注我领取海量matlab电子书和数学建模资料
相关文章
|
2月前
|
数据安全/隐私保护
耐震时程曲线,matlab代码,自定义反应谱与地震波,优化源代码,地震波耐震时程曲线
地震波格式转换、时程转换、峰值调整、规范反应谱、计算反应谱、计算持时、生成人工波、时频域转换、数据滤波、基线校正、Arias截波、傅里叶变换、耐震时程曲线、脉冲波合成与提取、三联反应谱、地震动参数、延性反应谱、地震波缩尺、功率谱密度
基于混合整数规划的微网储能电池容量规划(matlab代码)
基于混合整数规划的微网储能电池容量规划(matlab代码)
|
2月前
|
数据安全/隐私保护
地震波功率谱密度函数、功率谱密度曲线,反应谱转功率谱,matlab代码
地震波格式转换、时程转换、峰值调整、规范反应谱、计算反应谱、计算持时、生成人工波、时频域转换、数据滤波、基线校正、Arias截波、傅里叶变换、耐震时程曲线、脉冲波合成与提取、三联反应谱、地震动参数、延性反应谱、地震波缩尺、功率谱密度
|
2天前
|
传感器 算法
基于无线传感器网络的MCKP-MMF算法matlab仿真
MCKP-MMF算法是一种启发式流量估计方法,用于寻找无线传感器网络的局部最优解。它从最小配置开始,逐步优化部分解,调整访问点的状态。算法处理访问点的动态影响半径,根据带宽需求调整,以避免拥塞。在MATLAB 2022a中进行了仿真,显示了访问点半径请求变化和代价函数随时间的演变。算法分两阶段:慢启动阶段识别瓶颈并重设半径,随后进入周期性调整阶段,追求最大最小公平性。
基于无线传感器网络的MCKP-MMF算法matlab仿真
|
5天前
|
机器学习/深度学习 算法 数据挖掘
基于改进K-means的网络数据聚类算法matlab仿真
**摘要:** K-means聚类算法分析,利用MATLAB2022a进行实现。算法基于最小化误差平方和,优点在于简单快速,适合大数据集,但易受初始值影响。文中探讨了该依赖性并通过实验展示了随机初始值对结果的敏感性。针对传统算法的局限,提出改进版解决孤点影响和K值选择问题。代码中遍历不同K值,计算距离代价,寻找最优聚类数。最终应用改进后的K-means进行聚类分析。
|
6天前
|
算法 数据安全/隐私保护
基于GA遗传优化算法的Okumura-Hata信道参数估计算法matlab仿真
在MATLAB 2022a中应用遗传算法进行无线通信优化,无水印仿真展示了算法性能。遗传算法源于Holland的理论,用于全局优化,常见于参数估计,如Okumura-Hata模型的传播损耗参数。该模型适用于150 MHz至1500 MHz的频段。算法流程包括选择、交叉、变异等步骤。MATLAB代码执行迭代,计算目标值,更新种群,并计算均方根误差(RMSE)以评估拟合质量。最终结果比较了优化前后的RMSE并显示了SNR估计值。
21 7
|
3天前
|
算法
基于粒子群优化的图像融合算法matlab仿真
这是一个基于粒子群优化(PSO)的图像融合算法,旨在将彩色模糊图像与清晰灰度图像融合成彩色清晰图像。在MATLAB2022a中测试,算法通过PSO求解最优融合权值参数,经过多次迭代更新粒子速度和位置,以优化融合效果。核心代码展示了PSO的迭代过程及融合策略。最终,使用加权平均法融合图像,其中权重由PSO计算得出。该算法体现了PSO在图像融合领域的高效性和融合质量。
|
3天前
|
传感器 算法 数据安全/隐私保护
基于鲸鱼优化的DSN弱栅栏覆盖算法matlab仿真
```markdown 探索MATLAB2022a中WOA与DSN弱栅栏覆盖的创新融合,模拟鲸鱼捕食策略解决传感器部署问题。算法结合“搜索”、“包围”、“泡沫网”策略,优化节点位置以最大化复杂环境下的区域覆盖。目标函数涉及能量效率、网络寿命、激活节点数、通信质量及覆盖率。覆盖评估基于覆盖半径比例,旨在最小化未覆盖区域。 ```
|
9天前
|
算法 数据挖掘
MATLAB数据分析、从算法到实现
MATLAB数据分析、从算法到实现
|
15天前
|
机器学习/深度学习 算法 调度
Matlab|基于改进鲸鱼优化算法的微网系统能量优化管理matlab-源码
基于改进鲸鱼优化算法的微网系统能量管理源码实现,结合LSTM预测可再生能源和负荷,优化微网运行成本与固定成本。方法应用于冷热电联供微网,结果显示经济成本平均降低4.03%,提高经济效益。代码包括数据分段、LSTM网络定义及训练,最终展示了一系列运行结果图表。

热门文章

最新文章