✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现私信
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
1. 整体框架与功能概述
脚本目的：执行无人机包裹递送的完整任务模拟，包括场景搭建、任务规划、飞行模拟及结果展示。
所需工具箱：
UAV Toolbox、Navigation Toolbox和Aerospace Toolbox。
2. 参数定义 (params)
dt：仿真时间步长，设置为0.05秒，用于离散化时间推进模拟。mass：无人机质量，1.5千克。g：重力加速度，9.81m/s²。Ixx、Iyy、Izz：分别为无人机绕 x、y、z 轴的转动惯量。tau：速度响应时间常数，0.3秒，用于描述无人机速度对控制输入的响应快慢。cruiseAlt：巡航高度，25米，即无人机在飞行过程中保持的稳定高度。cruiseSpeed：巡航速度，6m/s，是无人机飞行的目标速度。lidarRange：激光雷达最大探测距离，50米。lidarRays：激光雷达发射光线数量，72条，对应5度的分辨率。safeRadius：避障激活半径，18米，当无人机距离障碍物小于此半径时启动避障机制。repGain：斥力势场增益，25，用于调整避障时的斥力大小。
3. 步骤 1：设置城市场景 (setup_scenario)
功能：创建一个包含建筑物的城市场景，用于无人机递送模拟。
返回值：
scene：uavScenario对象，用于 3D 可视化和激光雷达模拟。buildings：一个Nx5的矩阵，每一行代表一个建筑物，包含建筑物的中心坐标 (cx,cy)、半宽 (half_w)、半深 (half_d) 和高度 (height)。
场景构建细节：
使用
uavScenario创建场景，并设置更新速率、参考位置和停止时间。添加地面平面、道路网格以及各个建筑物的模型，同时为建筑物分配不同的颜色。
在原点和递送地点分别添加绿色和红色的平台。
4. 步骤 2：规划递送任务 (plan_mission)
功能：生成无人机包裹递送任务的航路点。
返回值：一个结构体
wpts，包含以下字段：positions：Nx3的矩阵，存储航路点的三维坐标[x y z]。toa：Nx1的向量，记录每个航路点的到达时间（秒）。totalTime：任务总时长（秒）。labels：单元格数组，包含每个航路点的名称。
任务规划细节：
定义了从仓库出发、起飞、前往取货点、悬停取货、前往送货点、悬停送货、返回仓库并降落的一系列航路点。
根据相邻航路点的距离和巡航速度计算每个航路点的到达时间，并考虑在取货和送货点的悬停时间。
5. 步骤 3：运行飞行模拟 (simulate_flight)
功能：使用导航工具箱的航路点轨迹、一阶速度模型和势场避障方法，运行无人机飞行模拟，并通过 2D 光线投射模拟激光雷达。
返回值：一个结构体
logs，记录了模拟过程中的各种数据，包括时间、位置、速度、参考位置、姿态、激光雷达扫描数据以及避障是否激活等信息。模拟过程细节：
从航路点轨迹获取参考位置和速度。
通过比例 - 前馈控制计算期望速度，并将其限制在巡航速度内。
使用势场避障方法调整期望速度，考虑与建筑物的距离并计算斥力。
根据一阶速度响应模型更新无人机的速度和位置，并确保高度不低于地面。
根据加速度计算无人机的近似姿态。
每
0.5秒进行一次激光雷达扫描模拟，并记录相关数据。记录当前时间步的各项数据，并通过进度指示符显示模拟进度。
根据给定的时间步长和任务总时长确定模拟的步数，并创建航路点轨迹对象。
初始化无人机的位置、速度和姿态。
在每个时间步中：
6. 步骤 4：可视化结果 (plot_results)
功能：代码中未给出
plot_results函数的具体实现，但从整体逻辑来看，它应该用于根据模拟记录的数据生成可视化结果，可能包括无人机的飞行轨迹、与建筑物的相对位置、激光雷达扫描结果等。
7. 其他函数
potential_field_avoidance：实现了基于斥力势场的避障算法。根据无人机当前位置和建筑物信息，计算斥力并调整期望速度，以避免与建筑物碰撞。如果无人机处于避障激活半径内，则标记为正在避障，并限制输出速度在1.5倍巡航速度内。simulate_lidar：通过光线与轴对齐包围盒（AABB）的相交检测，模拟 2D 水平激光雷达扫描。在水平平面上均匀发射nRays条光线，找到每条光线与最近建筑物墙壁的交点，记录距离和角度信息，并以结构体形式返回扫描结果。
⛳️ 运行结果
📣 部分代码
function plot_results(scene, logs, wpts, buildings, params)%PLOT_RESULTS Generate all mission visualisation figures.%% Figure 1: 3D mission view (scenario + flight path)% Figure 2: Position tracking (X, Y, Z vs time)% Figure 3: Speed profile + obstacle avoidance activity% Figure 4: Lidar obstacle map (last scan)% Figure 5: UAV attitude (roll, pitch, yaw vs time)speed = vecnorm(logs.vel, 2, 2);%% Figure 1: 3D Mission Viewfig1 = figure('Name','UAV Package Delivery - 3D Mission', ...'Position',[50 50 1100 750]);ax1 = axes(fig1);show3D(scene, 'Parent', ax1);hold(ax1, 'on');plot3(ax1, logs.pos(:,1), logs.pos(:,2), logs.pos(:,3), ...'r-', 'LineWidth', 2.5, 'DisplayName', 'Actual path');plot3(ax1, logs.ref(:,1), logs.ref(:,2), logs.ref(:,3), ...'b--', 'LineWidth', 1.5, 'DisplayName', 'Reference path');% Mark waypointsfor i = 1:size(wpts.positions,1)p = wpts.positions(i,:);if i == 1 || i == size(wpts.positions,1)plot3(ax1, p(1), p(2), p(3), 'k^', 'MarkerSize',14, ...'MarkerFaceColor','k', 'HandleVisibility','off');elseplot3(ax1, p(1), p(2), p(3), 'go', 'MarkerSize',10, ...'MarkerFaceColor','g', 'HandleVisibility','off');endtext(ax1, p(1)+3, p(2)+3, p(3)+3, wpts.labels{i}, ...'FontSize', 8, 'Color', 'w');end% Avoidance segmentsavoidIdx = find(logs.avoidActive);if ~isempty(avoidIdx)plot3(ax1, logs.pos(avoidIdx,1), logs.pos(avoidIdx,2), logs.pos(avoidIdx,3), ...'y.', 'MarkerSize', 6, 'DisplayName', 'Avoidance active');endxlabel(ax1,'X (m)'); ylabel(ax1,'Y (m)'); zlabel(ax1,'Z (m)');title(ax1,'UAV Package Delivery — 3D Mission View');legend(ax1, 'Location','northwest');grid(ax1,'on');view(ax1, 45, 30);axis(ax1, 'tight');%% Figure 2: Position Trackingfigure('Name','Position Tracking', 'Position',[100 100 950 650]);lbls = {'X (m)','Y (m)','Z (m)'};for k = 1:3subplot(3,1,k);plot(logs.t, logs.pos(:,k), 'r', 'LineWidth',1.5, 'DisplayName','Actual');hold on;plot(logs.t, logs.ref(:,k), 'b--','LineWidth',1.2, 'DisplayName','Reference');% Mark waypoint arrivalsfor w = 1:length(wpts.toa)xline(wpts.toa(w), 'k:', 'HandleVisibility','off');endylabel(lbls{k}); grid on;if k == 1legend('Location','northeast');title('Position Tracking — Actual vs Reference');endif k == 3, xlabel('Time (s)'); endend%% Figure 3: Speed & Avoidance Activityfigure('Name','Speed Profile', 'Position',[150 150 950 500]);yyaxis left;plot(logs.t, speed, 'b', 'LineWidth', 1.5);yline(params.cruiseSpeed, 'b--', 'Cruise speed', 'LabelVerticalAlignment','bottom');ylabel('Speed (m/s)');yyaxis right;area(logs.t, double(logs.avoidActive), ...'FaceColor',[1 0.5 0], 'FaceAlpha', 0.3, 'EdgeColor','none');ylabel('Avoidance active');yticks([0 1]); yticklabels({'No','Yes'});xlabel('Time (s)');title('UAV Speed Profile and Obstacle Avoidance Activity');grid on; legend('Speed','Cruise speed','Avoidance');%% Figure 4: Lidar Obstacle MaplastIdx = find(~cellfun(@isempty, logs.lidar), 1, 'last');if ~isempty(lastIdx)scan = logs.lidar{lastIdx};figure('Name','Lidar Obstacle Map', 'Position',[200 200 650 650]);theta = scan.angles;r = scan.ranges;[px, py] = pol2cart(theta, r);% Colour by rangescatter(px, py, 10, r, 'filled');colormap('jet'); cb = colorbar;cb.Label.String = 'Range (m)';hold on;plot(0, 0, 'rv', 'MarkerSize',14, 'MarkerFaceColor','r', ...'DisplayName', sprintf('UAV pos (%.0f, %.0f, %.0f m)', ...logs.pos(lastIdx,1), logs.pos(lastIdx,2), logs.pos(lastIdx,3)));% Show max-range arctArc = linspace(0, 2*pi, 200);plot(params.lidarRange*cos(tArc), params.lidarRange*sin(tArc), ...'k:', 'LineWidth',0.8, 'DisplayName','Max range');xlabel('X — UAV frame (m)'); ylabel('Y — UAV frame (m)');title(sprintf('Lidar Obstacle Map (t = %.1f s)', logs.t(lastIdx)));legend('Location','northwest'); axis equal; grid on;clim([0 params.lidarRange]);end%% Figure 5: Attitudefigure('Name','UAV Attitude', 'Position',[250 250 950 600]);attLabels = {'Roll (deg)','Pitch (deg)','Yaw (deg)'};for k = 1:3subplot(3,1,k);plot(logs.t, logs.attitude(:,k), 'LineWidth',1.5);yline(0,'k--','HandleVisibility','off');ylabel(attLabels{k}); grid on;if k == 1, title('Estimated UAV Attitude'); endif k == 3, xlabel('Time (s)'); endendend
