【UAV避障-3D VFH+】

加点油。。。。2026-01-27 19:42

该模型在仿真场景中为无人机实现了航点跟踪与避障功能，接收一组航点信息，并通过 三维向量场直方图改进算法（3D VFH+）规划出无障碍物的飞行路径。

控制行中直接运行命令：

matlab 复制代码 
>> Scenario = uavScenario("UpdateRate",100,"ReferenceLocation",[0 0 0]);
>> 
>> openExample('uav/UAVObstacleAvoidanceInSimulinkExample')





>> addMesh(Scenario,"cylinder",{[0 0 1] [0 .01]},[0 1 0]);
>> InitialPosition = [0 0 -7];
InitialOrientation = [0 0 0];
>> platUAV = uavPlatform("UAV",Scenario, ...
                      "ReferenceFrame","NED", ...
                      "InitialPosition",InitialPosition, ...
                      "InitialOrientation",eul2quat(InitialOrientation));
>> updateMesh(platUAV,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));
>> AzimuthResolution = 0.5;      
ElevationResolution = 2;
>> MaxRange = 7;
AzimuthLimits = [-179 179];
ElevationLimits = [-15 15];
>> LidarModel = uavLidarPointCloudGenerator("UpdateRate",10, ...
                                         "MaxRange",MaxRange, ...
                                         "RangeAccuracy",3, ...
                                         "AzimuthResolution",AzimuthResolution, ...
                                         "ElevationResolution",ElevationResolution, ...
                                         "AzimuthLimits",AzimuthLimits, ...
                                         "ElevationLimits",ElevationLimits, ...                                       
                                         "HasOrganizedOutput",true);
>> uavSensor("Lidar",platUAV,LidarModel, ...
          "MountingLocation",[0 0 -0.4], ...
          "MountingAngles",[0 0 180]);
>> show3D(Scenario);
>> show3D(Scenario);
>> ObstaclePositions = [10 0; 20 10; 10 20]; % Locations of the obstacles
ObstacleHeight = 15;                      % Height of the obstacles
ObstaclesWidth = 3;                       % Width of the obstacles

for i = 1:size(ObstaclePositions,1)
    addMesh(Scenario,"polygon", ...
        {[ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2; ...
        ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2], ...
        [0 ObstacleHeight]},0.651*ones(1,3));
end
show3D(Scenario);
legend("Start Position","Obstacles")
>> ObstaclePositions = [10 0; 20 10; 10 20]; % Locations of the obstacles
ObstacleHeight = 15;                      % Height of the obstacles
ObstaclesWidth = 3;                       % Width of the obstacles

for i = 1:size(ObstaclePositions,1)
    addMesh(Scenario,"polygon", ...
        {[ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2; ...
        ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2], ...
        [0 ObstacleHeight]},0.651*ones(1,3));
end
show3D(Scenario);
legend("Start Position","Obstacles")
>> open_system("ObstacleAvoidanceDemo.slx");
>> Waypoints = [InitialPosition; 0 20 -7; 20 20 -7; 20 0 -7];
>> for i = 2:size(Waypoints,1)
    addMesh(Scenario,"cylinder",{[Waypoints(i,2) Waypoints(i,1) 1] [0 0.1]},[1 0 0]);
end
show3D(Scenario);
hold on
plot3([InitialPosition(1,2); Waypoints(:,2)],[InitialPosition(1,2); Waypoints(:,1)],[-InitialPosition(1,3); -Waypoints(:,3)],"-g")
legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path"])
>> % Proportional Gains
Px = 6;
Py = 6;
Pz = 6.5;

% Derivative Gains
Dx = 1.5;
Dy = 1.5;
Dz = 2.5;

% Integral Gains
Ix = 0;
Iy = 0;
Iz = 0;

% Filter Coefficients
Nx = 10;
Ny = 10;
Nz = 14.4947065605712; 
>> UAVSampleTime = 0.001;
Gravity = 9.81;
DroneMass = 0.1;
>> out = sim("ObstacleAvoidanceDemo.slx");
>> out = sim("ObstacleAvoidanceDemo.slx");
>> hold on
points = squeeze(out.trajectoryPoints(1,:,:))';
plot3(points(:,2),points(:,1),-points(:,3),"-r");
legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path","UAV Trajectory"])

❤️创建搭载自定义激光雷达传感器并含障碍物的无人机仿真场景

💗创建仿真场景

创建无人机仿真场景并设置其本地原点

matlab 复制代码 
Scenario = uavScenario("UpdateRate",100,"ReferenceLocation",[0 0 0]);

添加标记以指示无人机的起始位姿。

matlab 复制代码 
addMesh(Scenario,"cylinder",{[0 0 1] [0 .01]},[0 1 0]);

💗定义无人机平台

在北东地（NED）坐标系中指定无人机的初始位置与姿态。

matlab 复制代码 
InitialPosition = [0 0 -7];
InitialOrientation = [0 0 0];

在情景中创建UAV平台

matlab 复制代码 
platUAV = uavPlatform("UAV",Scenario, ...
                      "ReferenceFrame","NED", ...
                      "InitialPosition",InitialPosition, ...
                      "InitialOrientation",eul2quat(InitialOrientation));

添加四旋翼无人机网格模型以实现可视化展示。

matlab 复制代码 
updateMesh(platUAV,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));

💗创建并挂载传感器模型

指定激光雷达的分辨率。

matlab 复制代码 
AzimuthResolution = 0.5;      
ElevationResolution = 2;

指定激光雷达的探测距离。

matlab 复制代码 
MaxRange = 7;
AzimuthLimits = [-179 179];
ElevationLimits = [-15 15];

创建统计传感器模型，为激光雷达生成点云数据。

matlab 复制代码 
LidarModel = uavLidarPointCloudGenerator("UpdateRate",10, ...
                                         "MaxRange",MaxRange, ...
                                         "RangeAccuracy",3, ...
                                         "AzimuthResolution",AzimuthResolution, ...
                                         "ElevationResolution",ElevationResolution, ...
                                         "AzimuthLimits",AzimuthLimits, ...
                                         "ElevationLimits",ElevationLimits, ...                                       
                                         "HasOrganizedOutput",true);

创建激光雷达传感器并将其搭载于四旋翼无人机上。

matlab 复制代码 
uavSensor("Lidar",platUAV,LidarModel, ...
          "MountingLocation",[0 0 -0.4], ...
          "MountingAngles",[0 0 180]);

使用3D函数显示场景

matlab 复制代码 
show3D(Scenario);

为场景添加障碍物

在场景中添加代表建筑物的长方体障碍物。

matlab 复制代码 
ObstaclePositions = [10 0; 20 10; 10 20]; % Locations of the obstacles
ObstacleHeight = 15;                      % Height of the obstacles
ObstaclesWidth = 3;                       % Width of the obstacles

for i = 1:size(ObstaclePositions,1)
    addMesh(Scenario,"polygon", ...
        {[ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)-ObstaclesWidth/2; ...
        ObstaclePositions(i,1)+ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2; ...
        ObstaclePositions(i,1)-ObstaclesWidth/2 ObstaclePositions(i,2)+ObstaclesWidth/2], ...
        [0 ObstacleHeight]},0.651*ones(1,3));
end
show3D(Scenario);
legend("Start Position","Obstacles")

❤️模型概述

本模型包含以下核心组件：

  • 无人机场景模块------配置场景并可视化飞行轨迹；
  • 航点跟踪与避障模块------实现带避障功能的航点跟踪；
  • 控制器与被控对象模块------无人机的位置控制器；
  • 控制面板------通过该面板启用/关闭避障功能，同时调整避障前瞻距离。
matlab 复制代码 
open_system("ObstacleAvoidanceDemo.slx");

💗无人机场景模块

场景模块可完成场景配置，并对障碍物、飞行轨迹及激光雷达点云数据进行可视化展示。


该子系统包含以下模块：

无人机场景配置------配置场景模块，将生成的场景用于仿真；

无人机场景运动读取------从场景中读取无人机当前状态；

无人机场景激光雷达------从场景中读取点云数据；

无人机场景运动写入------更新无人机新状态；

无人机场景观测窗------可视化无人机的飞行轨迹与激光雷达点云数据。

💗航点跟踪与避障

航点跟踪与避障子系统根据无人机当前状态和点云数据，求解无障碍物的期望位置与期望偏航角。

该子系统包含以下模块和子系统：

  • 航点跟踪器------沿下一航点方向为无人机计算前瞻点；
  • 避障模块------采用3D VFH+算法，求解无碰撞飞行所需的无障方向与偏航角，并更新航点跟踪器模块计算得到的前瞻点；
  • 转换子系统------控制飞行过程中避障功能的调用频率，同时完成其他数据类型及坐标变换的转换工作；
  • 前瞻距离------常量模块，其数值与期望方向的单位向量相乘后，叠加至无人机当前位置，以此计算期望位置；
  • 避障使能子系统------实现避障功能的开启与关闭；
  • 航点组------无人机规划飞行途经的所有航点集合，用于为无人机指定飞行航点。
matlab 复制代码 
Waypoints = [InitialPosition; 0 20 -7; 20 20 -7; 20 0 -7];

Add markers to indicate the waypoints.

matlab 复制代码 
for i = 2:size(Waypoints,1)
    addMesh(Scenario,"cylinder",{[Waypoints(i,2) Waypoints(i,1) 1] [0 0.1]},[1 0 0]);
end
show3D(Scenario);
hold on
plot3([InitialPosition(1,2); Waypoints(:,2)],[InitialPosition(1,2); Waypoints(:,1)],[-InitialPosition(1,3); -Waypoints(:,3)],"-g")
legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path"])

💗控制器与被控对象

控制器与被控对象子系统根据前瞻点生成控制指令，并更新无人机的状态。

该子系统包含以下模块：

  • 控制器------该子系统解算控制指令（横滚、俯仰、偏航、推力），驱动无人机飞向期望位置，采用多PID控制环实现位置控制。
  • 四旋翼飞行器被控对象------该制导模型模块根据控制指令更新无人机状态。
  • 转换模块------该子系统从无人机状态中提取位置与姿态信息，并完成数据格式及坐标变换，以适配可视化需求。

需配置控制器参数，此类参数基于试凑法整定，可通过调参实现更平稳的飞行效果。

matlab 复制代码 
% Proportional Gains
Px = 6;
Py = 6;
Pz = 6.5;

% Derivative Gains
Dx = 1.5;
Dy = 1.5;
Dz = 2.5;

% Integral Gains
Ix = 0;
Iy = 0;
Iz = 0;

% Filter Coefficients
Nx = 10;
Ny = 10;
Nz = 14.4947065605712;

为控制器和被控对象模块设定重力、无人机质量及采样时间参数。

matlab 复制代码 
UAVSampleTime = 0.001;
Gravity = 9.81;
DroneMass = 0.1;

💗 控制面板

开关用于启用或禁用由避障模块对前瞻点的更新功能。

滑动条用于调整计算前瞻点时所使用的前瞻距离。

  • 前瞻距离越大，无人机的飞行速度越快，但其与障碍物发生碰撞的风险也越高。
  • 前瞻距离越小，飞行速度则越慢，碰撞障碍物的风险也相应更低。

模型仿真

完成模型配置并运行，观察无人机的飞行运动状态。

  • 无人机将沿规划航点飞行并完成避障，仿真随即结束。
  • 可调整前瞻距离以改变无人机的飞行速度，
  • 也可修改避障模块的参数，观察飞行路径的变化规律。
matlab 复制代码 
out = sim("ObstacleAvoidanceDemo.slx");


可视化无人机无障飞行轨迹

绘制无人机实际飞行轨迹与规划航点，直观展示避障功能对无人机飞行的影响。

matlab 复制代码 
hold on
points = squeeze(out.trajectoryPoints(1,:,:))';
plot3(points(:,2),points(:,1),-points(:,3),"-r");
legend(["Start Position","Obstacles","","","Waypoints","","","Direct Path","UAV Trajectory"])
相关推荐
赫尔·普莱蒂科萨·帕塔2 小时前
智能体工程
人工智能·机器人·软件工程·agi
China_Yanhy3 小时前
入职 Web3 运维日记 · 第 8 日：黑暗森林 —— 对抗 MEV 机器人的“三明治攻击”
运维·机器人·web3
RobotNow7 小时前
优秀的机器人厂商集中平台哪家服务全面
机器人
aini_lovee8 小时前
MATLAB基于小波技术的图像融合实现
开发语言·人工智能·matlab
3GPP仿真实验室9 小时前
【Matlab源码】6G候选波形：OFDM-IM 增强仿真平台 DM、CI
开发语言·matlab·ci/cd
CS_Zero9 小时前
无人机板载计算机树莓派等各种派设置飞控数传
无人机·飞控
晚霞的不甘9 小时前
CANN × ROS 2：为智能机器人打造实时 AI 推理底座
人工智能·神经网络·架构·机器人·开源
RobotNow10 小时前
优秀的机器人厂商集中平台哪家案例多
机器人
RobotNow11 小时前
优秀的机器人厂商集中平台哪家效率高
机器人
搬砖者（视觉算法工程师）11 小时前
工具科普介绍之Spherasim：无人机与机器人开发领域的工具
机器人·无人机
热门推荐
01GitHub 镜像站点02Claude Code + GLM4.7 避坑指南：解决 Unable to connect to Anthropic services03OpenClaw Chrome扩展使用教程 - 浏览器中继控制04Linux下V2Ray安装配置指南05使用 1panel面板 部署 php网站06openclaw配置教程（linux+局域网ollama）07从零搭建一个 PHP 登录注册系统（含完整源码）08UV安装并设置国内源09Vue-skills的中文文档10Claude Code Skills 实用使用手册