【RRT三维路径规划】快速搜索随机树RRT多无人机UAV三维路径规划【含Matlab源码 3315期】

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（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算法仅依赖本身隐含的连接启发式贪婪搜索函数, 已能获得可行航迹, 虽然不是最优航迹, 且可行航迹点搜索时间成本较高, 但在不引入外部数学模型就能求解得出无人机的可行航迹, 这是该算法的一个显著特点;当引入数学模型求解无人机航迹规划问题后, 随机树对于航迹点的搜索将由之前的全空间贪婪搜索转为在满足数学模型约束函数的区域内贪婪搜索, 极大地减小了搜索空间范围, 同时节约了可行航迹点搜索时间, 并在一定程度上缩短了生成的可行航迹总长度。

2 RRT算法
RRT算法是一种基于树形结构的路径规划算法，它的全称是Rapidly-exploring Random Tree。该算法通过随机采样和树的生长来搜索可行路径，具有较高的搜索效率和可扩展性。RRT算法的基本思想是从起点开始，不断生成随机点并向目标点生长树，直到找到一条可行路径。在生长树的过程中，需要考虑障碍物的影响，以保证生成的路径是可行的。RRT算法的优点是可以处理高维空间和非线性约束，适用于各种类型的机器人和自由度。

2.1 RRT算法的步骤
RRT算法的步骤如下：
（1）初始化树，将起始点作为树的唯一节点。
（2）生成一个随机点，如果该点在障碍物内，则重新生成随机点。
（3）在树中找到距离随机点最近的节点。
从最近的节点向随机点生成一条路径，并检查路径是否与障碍物相交。
（4）如果路径不相交，则将路径的终点作为新的节点加入树中。
（5）重复步骤2-5，直到生成终点或达到最大迭代次数。
（6）如果生成了终点，则从终点开始沿着树的父节点一直回溯到起点，得到一条路径。

2.2 RRT算法无人机三维路径规划
RRT算法是一种常用的路径规划算法，它可以用于无人机三维路径规划。RRT算法的基本思想是通过随机采样和树的生长来搜索可行路径。具体来说，RRT算法通过在空间中随机采样一个点，然后在树中找到最近的节点，将这两个点之间的路径加入树中，不断重复这个过程，直到找到目标点或者达到最大迭代次数。在无人机三维路径规划中，RRT算法可以通过将空间划分为多个层次，每个层次对应一个高度范围，来实现无人机的三维路径规划。同时，RRT算法还可以通过引入启发式信息，如地形高度、风速等，来提高路径规划的效率和质量。

⛄二、部分源代码

clc;clear;close all;
dim = 3;
random_world = 0;

% create random world
world_size = 100;
NumObstacles = 100;
if random_world ==1
world = createWorld(NumObstacles,ones(1,dim)*world_size,zeros(1,dim));
else
[world,NumObstacles] = createKnownWorld(ones(1,dim)*world_size,[0;0;0],world_size);
end

UAV_num=10; % number of UAVs
start=zeros(UAV_num,3);goal=zeros(UAV_num,3);
path=cell(1,UAV_num);tree=cell(1,UAV_num);
inflec=cell(1,UAV_num);traj=cell(1,UAV_num);

% generate start & goal point
for i=1:UAV_num
start(i,1)=irand;goal(i,1)=irand+(world_size-UAV_num);
start(i,2)=irand;goal(i,2)=irand+(world_size-UAV_num);
start(i,3)=irand;goal(i,3)=irand+(world_size-UAV_num);
end
% generate RRT path
for i=1:UAV_num
[path{i},tree{i}]=getPath(start(i,:), goal(i,:), world);
[inflec{i},~,traj{i}]=getTrajectory(path{i});
% % draw RRT result
% figure
% plotExpandedTree(world,tree{i},3);
% plotWorld(world,path{i},1);
end

figure
for i=1:UAV_num
plotWorld(world,path{i},2);
hold on
end

UAVsize=1;colLimit=3*UAVsize;
[newTrajectory, coltime]=multiDetCon(traj,inflec,colLimit);
getGIFTra(newTrajectory);
% twoTrajDis(newTrajectory{1},newTrajectory{2},colLimit);

% multiTrajInflec(traj);
% multiTrajInflec(inflec);
%
% UAVsize=1;colLimit=3*UAVsize;
% [colnum,coltime]=timeCol(traj{1},traj{2},colLimit);
% [~,newTraj]=slowTrajectory(inflec{2},coltime);
% showOldNew(traj{2},newTraj,2);
%
% twoTrajDis(traj{1},newTraj);
% [newcolnum, newcoltime]=timeCol(traj{1},newTraj,colLimit);

function [path,tree]=getPath(start_cord, goal_cord, world)
% standard length of path segments
dim=3;segmentLength = 5;
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];

% establish tree starting with the start node
tree = start_node;

% check to see if start_node connects directly to end_node
if ((norm(start_node(1:dim)-end_node(1:dim))<segmentLength)&&(collision(start_node,end_node,world,dim)==0))
path = [start_node; end_node];
else
nIterations = 0;
numPaths = 0;
flag = 0;
while numPaths<1
[tree,flag] = extendTree(tree,end_node,segmentLength,world,dim);
numPaths = numPaths + flag;
nIterations = nIterations+1;
end
end

⛄三、运行结果

⛄四、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 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合