【RRT三维路径规划】基于matlab改进RRT算法无人机三维路径规划【含Matlab源码 2936期】

💥💥💥💥💞💞💞💞💞💞欢迎来到凤凰科研社博客之家💞💞💞💞💞💞💥💥💥💥

✅博主简介：985研究生，热爱科研的Matlab仿真开发者，完整代码 论文复现 程序定制 期刊写作 科研合作 扫描文章底部QQ二维码。
🍎个人主页：凤凰科研社
🏆代码获取方式：扫描文章底部QQ二维码

⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（凤凰科研社版）

⛳️关注微信公众号Matlab王者助手或Matlab海神之光，更多资源等你来！！

⛄一、快速扩展随机树无人机三维路径规划简介

1 无人机航迹规划问题的数学模型
建立三维航迹规划问题的数学模型时, 不但考虑无人机基本约束, 还考虑复杂的飞行环境, 包括山体地形和雷暴威胁区。

1.1 无人机基本约束
规划的无人机三维航迹, 通常需要满足一些基本约束, 包括最大转弯角、最大爬升角或下滑角、最小航迹段长度、最低和最高飞行高度, 以及最大航迹长度等约束。其中, 最大转弯角约束, 是指无人机只能在水平面内小于或等于指定的最大转弯角内转弯;最大爬升角或下滑角约束, 是指无人机只能在垂直平面内小于或等于指定的最大爬升角或下滑角内爬升或下滑;最小航迹段长度约束, 要求无人机改变飞行姿态之前, 按目前的航迹方向飞行的最短航程;最低和最高飞行高度约束, 要求无人机在指定的飞行高度区间飞行;最大航迹长度约束, 是指无人机的航迹长度小于或等于指定的阈值。

记q (x, y, z, θ, ψ) 为无人机的飞行位置与姿态, 其中, (x, y, z) 为无人机的位置, θ为无人机的水平转弯角, ψ为无人机的竖直爬升角或下滑角, 进而建立上述基本约束的数学表达式。

1.2 飞行环境障碍物和威胁区建模
在飞行环境中, 高耸的山体近似采用圆锥体等效表示, 用以e为底的自然指数图形生成, 那么, 山体地形可以通过多个位置不同的圆锥体叠加而成。若将参考海拔基准高度设置为xOy平面, 记 (x, y, z) 为山体地形中的点, 那么

式中:N为山体个数;xk0和yk0为第k座山体中心对称轴的横坐标和纵坐标;hk为第k座山体的最大高度;xki和yki为第k座山体的横向斜度和纵向斜度。

在飞行环境中, 山体附近通常存在雷暴等极端气象, 本文视为飞行威胁区, 并通过球体近似等效表示, 且记第k座山体附近飞行威胁区的球心坐标为 (xks0, yks0, zks0) , 半径为rk。

1.3 目标函数及航迹表示
在本文中, 执行任务的某型无人机, 其航迹规划的目标函数是生成一条由起始点到目标点的无碰撞可行航迹。采用q (x, y, z, θ, ψ) 表示无人机在飞行空域中某特定位置的特定姿态, 那么 (x, y, z) 则表示无人机所在航迹点, θ表示无人机的水平转弯角, ψ表示无人机的竖直爬升角或下滑角。采用r (q) 表示由起始点qinitial到目标点qgoal的无碰撞可行航迹, 那么航迹规划的过程可以写成如下形式:

1.4 数学模型表述
基于1.1~1.3节, 得到无人机航迹规划问题的数学模型。

目标函数:

经典RRT-Connect算法仅依赖本身隐含的连接启发式贪婪搜索函数, 已能获得可行航迹, 虽然不是最优航迹, 且可行航迹点搜索时间成本较高, 但在不引入外部数学模型就能求解得出无人机的可行航迹, 这是该算法的一个显著特点;当引入数学模型求解无人机航迹规划问题后, 随机树对于航迹点的搜索将由之前的全空间贪婪搜索转为在满足数学模型约束函数的区域内贪婪搜索, 极大地减小了搜索空间范围, 同时节约了可行航迹点搜索时间, 并在一定程度上缩短了生成的可行航迹总长度。

⛄二、部分源代码

% =使用贪婪算法平滑
clear all
clc
format long
close all

%% 环境生成
%------单起点-单目标点类型------
start = [900 900 150];
goal = [500 700 450];
Task.IniState = start ;
Task.FinState = goal ;
%=============================
%---------单起点-多目标点类型-------
% start = [900 900 180];
% goal = [500 700 340];
% Task.IniState = start ;
% Task.FinState = goal ;
% Task.SedState = [700 800 260] ;
%================================
%---------多起点-多目标点类型-------
% start = [900 900 150];
% goal = [520 780 400];
% Task.IniState = start ;
% Task.FinState = goal ;
% Task.SedState = [850 750 240] ;
% Task.Sedfin = [500 700 340] ;
%================================
Task.Boundary = reshape([ min(min(start).*ones(1,3),min(goal).*ones(1,3));max(max(start).ones(1,3),max(goal).ones(1,3))],[1,6]) ;
Task.Boundary(6) = goal(3) ;
Task.Timeinte = [ 0 , 1000 ] ;
Task.DisScale = [900,900] ; % 环境参考点数
Task.Peaks = [413,275,689,469,800,0;…
248,496,441,717,0 ,200] ; % 列数为山峰个数
Task.Peaks(3,:) = [400,300,300,150,400,200] ; % 山峰高度
Task.Peaks(4,:) = 150 ones(1,size(Task.Peaks,2)) ; % 山峰陡峭度 X
Task.Peaks(5,:) = 120 ones(1,size(Task.Peaks,2)) ; % 山峰陡峭度 Y

% 生成山峰环境
PeakData = GenerateEnvironment_Peak(Task) ;
Task.Boundary(6) = max(max(max(PeakData.H)),Task.Boundary(6)) ;

load(‘data.mat’,‘data’)
PeakData.H = data ;

figure(3)
hold on
grid off
box on
axis equal
% view(73,34)
view(100,50)
colormap(‘summer’) %颜色表
% colormap(‘parula’)
surf(PeakData.X,PeakData.Y,PeakData.H,‘EdgeColor’,‘interp’)

% % ----------单起点-单目标点类型--------------
plot3(Task.IniState(1),Task.IniState(2),Task.IniState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,2)
plot3(Task.FinState(1),Task.FinState(2),Task.FinState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,2)
%=============================================

% ----------单起点-多目标点类型--------------
% plot3(Task.IniState(1),Task.IniState(2),Task.IniState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.SedState(1),Task.SedState(2),Task.SedState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.FinState(1),Task.FinState(2),Task.FinState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
%=============================================

% ----------多起点-多目标点类型--------------
% plot3(Task.IniState(1),Task.IniState(2),Task.IniState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.FinState(1),Task.FinState(2),Task.FinState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.SedState(1),Task.SedState(2),Task.SedState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.Sedfin(1),Task.Sedfin(2),Task.Sedfin(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
%=============================================

%% 传统 RRT 规划
cons = [inf,inf,inf] ;
rou = 0.3 ;
%-------------多起点-多目标点类型-----------------
% start = [900 900 150];
% goal = [520 780 400];
% path1 = RRTplan(start,goal,PeakData,rou,Task,‘g’,‘y’);
%
% start = [850 750 240];
% goal = [500 700 340];
% path2 = RRTplan(start,goal,PeakData,rou,Task,‘g’,‘y’);
%
% newPath1 = GreedyOptimize(path1,PeakData,Task,cons) ; % 贪婪算法平滑处理
% path3 = newPath1 ;
% newPath2 = GreedyOptimize(path2,PeakData,Task,cons) ;
% path4 = newPath2 ;

%-------------------------------------------------

%--------------------单起点-多目标点类型-----------
% start = [900 900 180];
% goal = [700 800 260] ;
% path1 = RRTplan(start,goal,PeakData,rou,Task,‘b’,‘y’);
%
% start = [700 800 260] ;
% goal = [500 700 340];
% path2 = RRTplan(start,goal,PeakData,rou,Task,‘b’,‘y’);
% path = [path1;path2];
%
% newPath1 = GreedyOptimize(path1,PeakData,Task,cons) ;
% newPath2 = GreedyOptimize(path2,PeakData,Task,cons) ;
% path3 = [ newPath1 ; newPath2] ;
%-------------------------------------------------

%--------------------单起点-单目标点类型-----------
start = [900 900 150];
goal = [500 700 450];
path1 = RRTplan(start,goal,PeakData,rou,Task,‘b’,‘y’);

newPath = GreedyOptimize(path1,PeakData,Task,cons) ;
path = newPath;

% tplot = linspace(path1(1,3),path1(end,3),10) ;
% pplot = spline(path1(:,3)‘,path1(:,1:3)’,tplot) ;
% tplot2 = linspace(tplot(1),tplot(end),100) ; T(start,goal,PeakData,rou,Task);

%绘制原RRT树
figure(4)
hold on
grid off
box on
axis equal
% view(73,34)
view(80,80)
% colormap(‘parula’)
colormap(‘summer’)
surf(PeakData.X,PeakData.Y,PeakData.H,‘EdgeColor’,‘interp’)

% -----------单起点-单目标点类型---------
plot3(Task.IniState(1),Task.IniState(2),Task.IniState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,2)
plot3(Task.FinState(1),Task.FinState(2),Task.FinState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,2)
plot3(path1(:,1), path1(:,2), path1(:,3),‘LineWidth’,2,‘Color’,‘b’);
plot3(path(:,1), path(:,2), path(:,3),‘LineWidth’,2,‘Color’,‘r’);
p = path(:,1:3)’ ;
disp([‘总航程为 ‘,num2str(sum(sum(sqrt((p(:,2:end) - p(:,1:end-1)).^2)))*1e-3),’ km’])

%====================================================================================

%-----------单起点-多目标点类型---------
% Task.SedState = [700 800 260] ;
% plot3(Task.IniState(1),Task.IniState(2),Task.IniState(3),‘pg’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.SedState(1),Task.SedState(2),Task.SedState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
% plot3(Task.FinState(1),Task.FinState(2),Task.FinState(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
%
% plot3(path(:,1), path(:,2), path(:,3),‘LineWidth’,1,‘Color’,‘b’);
% plot3(path3(:,1), path3(:,2), path3(:,3),‘LineWidth’,1,‘Color’,‘r’);
%
% p = path3(:,1:3)’ ;
% disp([‘总航程为 ‘,num2str(sum(sum(sqrt((p(:,2:end) - p(:,1:end-1)).^2)))*1e-3),’ km’])
%====================================================================================

%-----------多起点-多目标点类型---------
% Task.SedState = [850 750 240] ;
% Task.Sedfin = [500 700 340] ;

% plot3(Task.Sedfin(1),Task.Sedfin(2),Task.Sedfin(3),‘pr’,‘MarkerSize’,15,‘Linewidth’,1)
%
% plot3(path1(:,1), path1(:,2), path1(:,3),‘LineWidth’,1,‘Color’,‘b’);
% plot3(path3(:,1), path3(:,2), path3(:,3),‘LineWidth’,1,‘Color’,‘r’);
% plot3(path2(:,1), path2(:,2), path2(:,3),‘LineWidth’,1,‘Color’,‘b’);
% plot3(path4(:,1), path4(:,2), path4(:,3),‘LineWidth’,1,‘Color’,‘r’);
%
% p = path2(:,1:3)’ ;
% disp([‘1的总航程为 ‘,num2str(sum(sum(sqrt((p(:,2:end) - p(:,1:end-1)).^2)))*1e-3),’ km’])
% p2 = path4(:,1:3)’ ;
% disp([‘2的总航程为 ‘,num2str(sum(sum(sqrt((p2(:,2:end) - p2(:,1:end-1)).^2)))*1e-3),’ km’])
%====================================================================================

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]田疆,李二超.用于无人机三维航迹规划改进连接型快速扩展随机树算法[J].航空工程进展. 2018,9(04)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合