【UAV避障-3D VFH+】

该模型在仿真场景中为无人机实现了航点跟踪与避障功能，接收一组航点信息，并通过三维向量场直方图改进算法（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"])