✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥内容介绍
在科技飞速发展的当下，无人机凭借其独特优势，已广泛应用于众多领域，成为推动各行业变革与发展的关键力量。在军事领域，无人机承担着侦察、目标定位和情报收集等重要任务，为军事决策提供了关键支持，显著提升了作战的灵活性与隐蔽性 。在民用方面，无人机的身影同样无处不在。在农业植保中，它能高效地进行农药喷洒和病害监测，大幅提高作业效率的同时，减少了农药使用量和人力投入，有力推动了农业的现代化进程；在物流配送领域，无人机有望突破传统物流的瓶颈，实现 “最后一公里” 的快速配送，尤其在偏远地区或紧急物资运输中，展现出无可替代的优势；在环境监测领域，无人机可搭载专业设备，对大气、水质、土壤等进行实时监测，为环境保护和生态研究提供了大量的数据支持 。
然而，无论在何种应用场景下，无人机要高效、安全地完成任务，路径规划都起着举足轻重的作用。路径规划就如同为无人机绘制了一张精确的 “行军路线图”，它能帮助无人机巧妙地避开障碍物和禁飞区，避免发生碰撞事故，确保飞行安全。同时，合理的路径规划还能使无人机以最短的飞行距离、最少的能耗和最短的时间抵达目标地点，极大地提高了任务执行的效率。例如，在物流配送中，精准的路径规划可以让无人机快速送达货物，节省时间和成本；在应急救援中，优化后的路径能使无人机迅速抵达受灾区域，为救援工作争取宝贵的时间 。可以说，路径规划是无人机实现智能化、自主化飞行的核心技术，直接关系到无人机在各领域应用的成效与发展前景。
传统 RRT 算法原理及不足
传统 RRT 算法详解
快速探索随机树（RRT，Rapidly-Exploring Random Tree）算法，作为路径规划领域的经典算法，自 1998 年由 Steven M. LaValle 提出后，便以其独特的优势在机器人运动规划、自动驾驶等诸多领域得到了广泛应用 。其核心思想是通过在状态空间中进行随机采样，逐步构建一棵搜索树，以快速找到从起点到终点的可行路径。
该算法的核心步骤主要包含以下几个方面：
1.初始化：将起点作为树的根节点，构建初始的搜索树。此时，这棵树仅包含一个节点，就像一颗刚刚种下的种子，等待着在搜索空间中生根发芽 。
2.随机采样：在整个搜索空间内随机生成一个采样点。这个随机点的产生就像是在一片广阔的草原上随机选取一个位置，为后续的搜索指明方向。例如，在一个二维平面的搜索空间中，随机采样点可能是坐标为 (3, 5) 的点
创建连接边：从最近邻居节点向随机采样点方向，按照一定的步长进行扩展，生成一个新的节点，并将新节点与最近邻居节点连接起来，形成一条边。步长的大小是一个关键参数，它直接影响着搜索的效率和路径的质量。如果步长设置过小，算法的搜索速度会变慢，需要更多的迭代次数才能找到路径；而步长设置过大，则可能会导致错过一些可行的路径，甚至直接跨过障碍物 。
5.碰撞检测：检查新生成的路径（即从最近邻居节点到新节点的连线）是否与障碍物发生碰撞。若发生碰撞，则舍弃该新节点；若未碰撞，则将新节点添加到搜索树中。这一步骤就像是在前进的道路上，检查是否会撞到障碍物，如果会撞到，就需要重新寻找其他路线 。
6.目标检测：不断重复上述步骤，持续扩展搜索树，直到新生成的节点进入目标点的一定邻域范围内，此时认为找到了一条从起点到终点的可行路径。这个邻域范围可以根据实际需求进行设定，例如可以设定为以目标点为圆心，半径为 5 的圆形区域 。
为了更直观地理解 RRT 算法的工作过程，我们来看一个简单的示意图。假设在一个二维平面中，存在一些障碍物（用黑色区域表示），起点为蓝色点，终点为绿色点 。首先，将起点作为根节点，开始构建搜索树。然后，随机生成一个采样点（用红色三角形表示），找到树中距离该采样点最近的节点（此时树中只有起点节点），从起点向采样点方向扩展一个步长，生成新节点（用黄色圆圈表示），检查新节点与起点之间的连线是否与障碍物碰撞，若不碰撞，则将新节点添加到树中。不断重复这个过程，随着搜索树的不断扩展，最终找到一条从起点到终点的可行路径（用红色线段表示） 。通过这样的方式，RRT 算法能够在复杂的环境中，快速地探索出一条可行的路径，为后续的路径规划和优化奠定了基础。
传统 RRT 算法的缺陷
尽管传统 RRT 算法在路径规划领域展现出了强大的探索能力，能够在复杂环境中快速找到一条可行路径，但其自身也存在一些不容忽视的缺陷，在实际应用中可能会受到一定的限制 。
路径非最优：传统 RRT 算法的目标主要是快速找到一条从起点到终点的可行路径，而不是追求最优路径。这是因为它在搜索过程中采用的是随机采样策略，缺乏对全局信息的有效利用。例如，在图 1 的场景中，RRT 算法找到的路径可能是一条较为曲折的路线（如图中红色路径所示），而实际上可能存在一条更短、更优的路径（如图中绿色路径所示）。这是由于随机采样点的不确定性，使得算法在扩展搜索树时，无法保证每次都朝着最优路径的方向进行扩展，从而导致最终得到的路径往往不是最优的 。这种非最优路径在实际应用中可能会带来一系列问题。以无人机配送为例，如果无人机按照非最优路径飞行，会增加飞行距离和时间，从而导致能耗增加，降低配送效率。此外，较长的飞行路径也可能增加无人机与障碍物碰撞的风险，影响飞行安全 。
盲目搜索，效率较低：传统 RRT 算法在搜索空间中进行随机采样时，缺乏对搜索方向的有效引导，存在一定的盲目性。这意味着它可能会在一些没有希望找到路径的区域进行大量无效采样，从而浪费了计算资源和时间。比如，在一个存在大量障碍物的环境中，随机采样点可能会频繁地落在障碍物区域或者远离目标点的区域，使得搜索树的扩展变得缓慢且低效。而且，在搜索过程中，RRT 算法对于已经探索过的区域和未探索过的区域缺乏区分和重点关注，不能根据已有的信息来优化后续的采样策略，导致搜索效率难以得到有效提升 。在时间敏感的应用场景中，如应急救援中，无人机需要尽快抵达受灾区域，传统 RRT 算法的低搜索效率可能会延误救援时机，造成严重的后果 。
对复杂环境适应性有限：当面对复杂的环境，如狭窄通道、密集障碍物等情况时，传统 RRT 算法的性能会受到严重影响。在狭窄通道中，由于通道宽度有限，随机采样点命中通道区域的概率较低，使得搜索树很难在该区域进行有效扩展，导致算法可能长时间无法找到通过狭窄通道的路径。在密集障碍物环境中，大量的障碍物增加了碰撞检测的计算量和难度，同时也增加了搜索树扩展的难度，使得算法的收敛速度变慢，甚至可能陷入局部最小值，无法找到全局最优解 。在城市环境中进行无人机路径规划时，高楼大厦等障碍物密集分布，传统 RRT 算法可能难以快速找到一条安全、高效的飞行路径 。这些缺陷限制了传统 RRT 算法在一些对路径质量和搜索效率要求较高的场景中的应用，促使研究人员不断探索改进算法，以提高其性能和适应性 。
带信息的 RRT 算法
带信息 RRT 算法的创新之处
带信息的 RRT 算法，全称为 Informed RRT 算法，作为对传统 RRT 算法的重大改进，在路径规划领域展现出了独特的创新优势 。它的核心创新在于巧妙地引入了先验信息和启发式函数，以此来引导搜索方向，极大地提升了搜索效率和路径质量 。
在引入先验信息方面，带信息的 RRT 算法能够充分利用已知的环境信息，如地图数据、障碍物分布规律等，来指导搜索过程。以在城市环境中进行无人机路径规划为例，算法可以根据预先获取的城市地图，了解高楼大厦等障碍物的位置和形状，从而在搜索时避免在障碍物密集区域进行无效采样 。通过对地图中开阔区域和禁飞区的分析，算法能够有针对性地选择采样点，使得搜索树优先向可行区域扩展，减少了在不可行区域的探索时间，提高了搜索效率 。这种对先验信息的有效利用，就像是为无人机配备了一位经验丰富的向导，能够引导它在复杂的城市环境中快速找到安全的飞行路径 。
启发式函数的运用也是带信息 RRT 算法的一大亮点。启发式函数能够根据当前节点与目标点之间的距离、方向等信息，对节点的优先级进行评估 。在搜索过程中，算法会优先选择那些启发式函数值更优的节点进行扩展，从而使搜索树更有方向性地朝着目标点生长 。常见的启发式函数如欧几里得距离启发式函数，通过计算节点与目标点之间的欧几里得距离，为每个节点赋予一个启发式值 。距离目标点越近的节点，其启发式值越低，优先级越高，算法会优先扩展这些节点 。这种基于启发式函数的节点选择策略，使得算法能够更快地找到靠近目标点的路径，减少了搜索的盲目性，提高了路径规划的效率和质量 。 可以说，带信息的 RRT 算法通过引入先验信息和启发式函数，打破了传统 RRT 算法的盲目搜索局限，为无人机在复杂环境中的路径规划提供了更智能、更高效的解决方案 。
⛳️ 运行结果
📣 部分代码
function [F, M, trpy, drpy] = controller(qd, t, qn, params)
% CONTROLLER quadrotor controller
% The current states are:
% qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega
% The desired states are:
% qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des
% Using these current and desired states, you have to compute the desired controls
% position controller params
Kp = [15;15;30];
Kd = [12;12;10];
% attitude controller params
KpM = ones(3,1)*3000;
KdM = ones(3,1)*300;
acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos);
% Desired roll, pitch and yaw
phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des));
theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des));
psi_des = qd{qn}.yaw_des;
euler_des = [phi_des;theta_des;psi_des];
pqr_des = [0;0; qd{qn}.yawdot_des];
% Thurst
qd{qn}.acc_des(3);
F = params.mass*(params.grav + acc_des(3));
% Moment
M = params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler));
% =================== Your code ends here ===================
% Output trpy and drpy as in hardware
trpy = [F, phi_des, theta_des, psi_des];
drpy = [0, 0, 0, 0];
end
🔗 参考文献
[1]郭聪.基于RRT的无人机三维航迹规划算法研究[D].沈阳航空航天大学,2015.
🎈 部分理论引用网络文献，若有侵权联系博主删除
👇 关注我领取海量matlab电子书和数学建模资料
🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：
🌟 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位、冷链、时间窗、多车场等、选址优化、港口岸桥调度优化、交通阻抗、重分配、停机位分配、机场航班调度、通信上传下载分配优化
🌟 机器学习和深度学习时序、回归、分类、聚类和降维
2.1 bp时序、回归预测和分类
2.2 ENS声神经网络时序、回归预测和分类
2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类
2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类
2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类
2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类
2.7 ELMAN递归神经网络时序、回归\预测和分类
2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类
2.9 RBF径向基神经网络时序、回归预测和分类
2.10 DBN深度置信网络时序、回归预测和分类
2.11 FNN模糊神经网络时序、回归预测
2.12 RF随机森林时序、回归预测和分类
2.13 BLS宽度学习时序、回归预测和分类
2.14 PNN脉冲神经网络分类
2.15 模糊小波神经网络预测和分类
2.16 时序、回归预测和分类
2.17 时序、回归预测预测和分类
2.18 XGBOOST集成学习时序、回归预测预测和分类
2.19 Transform各类组合时序、回归预测预测和分类
方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断
🌟图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知
🌟 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、 充电车辆路径规划（EVRP）、 双层车辆路径规划（2E-VRP）、 油电混合车辆路径规划、 船舶航迹规划、 全路径规划规划、 仓储巡逻、公交车时间调度、水库调度优化、多式联运优化
🌟 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划、
🌟 通信方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配
🌟 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测
🌟电力系统方面
微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电、电/冷/热负荷预测、电力设备故障诊断、电池管理系统（BMS）SOC/SOH估算（粒子滤波/卡尔曼滤波）、 多目标优化在电力系统调度中的应用、光伏MPPT控制算法改进（扰动观察法/电导增量法）、电动汽车充放电优化、微电网日前日内优化、储能优化、家庭用电优化、供应链优化\智能电网分布式能源经济优化调度，虚拟电厂，能源消纳，风光出力，控制策略，多目标优化，博弈能源调度，鲁棒优化
电力系统核心问题经济调度：机组组合、最优潮流、安全约束优化。新能源消纳：风光储协同规划、弃风弃光率量化、爬坡速率约束建模多能耦合系统：电-气-热联合调度、P2G与储能容量配置新型电力系统关键技术灵活性资源：虚拟电厂、需求响应、V2G车网互动、分布式储能优化稳定与控制：惯量支撑策略、低频振荡抑制、黑启动预案设计低碳转型：碳捕集电厂建模、绿氢制备经济性分析、LCOE度电成本核算风光出力预测：LSTM/Transformer时序预测、预测误差场景生成（GAN/蒙特卡洛）不确定性优化：鲁棒优化、随机规划、机会约束建模能源流分析、PSASP复杂电网建模，经济调度，算法优化改进，模型优化，潮流分析，鲁棒优化，创新点，文献复现微电网配电网规划，运行调度，综合能源，混合储能容量配置，平抑风电波动，多目标优化，静态交通流量分配，阶梯碳交易，分段线性化，光伏混合储能VSG并网运行，构网型变流器， 虚拟同步机等包括混合储能HESS：蓄电池+超级电容器，电压补偿,削峰填谷，一次调频，功率指令跟随，光伏储能参与一次调频，功率平抑，直流母线电压控制；MPPT最大功率跟踪控制，构网型储能，光伏，微电网调度优化，新能源，虚拟同同步机，VSG并网，小信号模型
🌟 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长 金属腐蚀
🌟 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合、SOC估计、阵列优化、NLOS识别
🌟 车间调度
零等待流水车间调度问题NWFSP、置换流水车间调度问题PFSP、混合流水车间调度问题HFSP、零空闲流水车间调度问题NIFSP、分布式置换流水车间调度问题 DPFSP、阻塞流水车间调度问题BFSP
👇
