旋翼飞控避障系统设计方案

1 概述

2 需求分析

3 功能性能指标

3.1 功能指标

3.2 性能指标

4 方案设计

4.1 软件设计方案

4.1.1 局部路径规划总体设计方案

4.1.2 全局路径规划总体设计方案

4.1.3 局部路径规划详细设计

1. 接收深度相机数据：深度相机提供高精度的3D环境感知数据，这些数据包含了障碍物的距离信息和空间分布。通过接收深度相机数据，规划器能够实时获得无人机前方的环境信息，用于进行避障决策。

2. 接收激光雷达数据：激光雷达数据与深度相机数据互为补充，提供了水平平面的环境信息，可以有效检测水平障碍物和地面特征。激光雷达的数据更新频率较高，适合对动态障碍物进行快速检测。

3. 将数据转换为局部坐标系：将深度相机和激光雷达数据转换为局部坐标系，以统一不同传感器的数据格式。转换后，所有的障碍物数据都以无人机自身为参考，从而更方便进行路径规划和避障计算。

1. 融合多源点云数据：融合来自不同传感器的数据（如深度相机和激光雷达），确保生成的点云数据更全面地反映环境的状况。融合后的数据经过滤波和去噪处理，以减少传感器噪声对路径规划的影响。

2. 更新环境网格：利用融合后的点云数据更新局部环境网格，环境网格表示为一个三维的栅格地图，其中每个单元格包含了当前的障碍物状态。这样，局部规划器可以根据网格信息进行路径计算。

1. 计算3D点云的俯仰角和航向角：对每个点云数据点，计算它相对于无人机的俯仰角和航向角。这些角度信息能够帮助直方图以极坐标的形式表示障碍物在三维空间中的位置，从而简化避障计算。

2. 生成直方图并二值化：将俯仰角和航向角信息转化为极坐标直方图，并对直方图进行二值化处理，将障碍物的存在与否表示为0或1。这样可以简化障碍物判断逻辑，并加快后续的路径规划计算。

1. 重投影前一时刻的直方图：将上一时刻的直方图重投影到当前无人机的位置。这一步能够保留无人机之前探测到的障碍物信息，尤其是当前视野外但仍可能对飞行产生影响的障碍物。

2. 与当前直方图融合：将重投影的历史直方图与当前时刻的直方图进行融合。融合过程确保无人机能够利用之前的环境数据，提高对当前环境的认知能力，避免因视野限制导致的路径选择错误。

3. 生成组合直方图：融合后的直方图即为组合直方图，用于描述当前及之前环境中的障碍物信息，从而帮助无人机在复杂环境中做出更稳定的避障决策。

1. 计算与目标的距离：计算无人机当前位置到目标位置的距离，用于评估每条候选路径的效果。距离越短的路径通常代价越低，有助于无人机尽快到达目标。

2. 计算路径平滑度：评估路径的平滑度，包括路径中转弯的数量和角度。路径越平滑，意味着飞行的稳定性越高，避免频繁的转向和加速度变化，从而提高飞行的安全性和舒适性。规划器会计算每条候选路径中的转弯次数及转弯角度，综合评估路径的平滑度得分，越平滑的路径代价越低。

3. 计算与障碍物的距离：计算无人机与周围障碍物之间的距离，以确保飞行路径远离潜在的碰撞风险。通过考虑与障碍物的距离，规划器能够有效避免危险区域，路径离障碍物越远，代价越低。这一步通过在组合直方图中查找占用的单元格来确定障碍物的位置，然后计算无人机与这些障碍物的最小距离，从而评估路径的安全性。

4. 综合计算代价：结合上述所有计算结果，包括与目标的距离、路径平滑度和与障碍物的距离，规划器将综合计算每条候选路径的代价。代价函数通过加权方式对不同因素进行评估，以平衡路径的安全性和到达目标的效率。权重因子可以根据实际应用的需求进行调节，以实现不同的飞行目标（如快速到达目标点或保持更高的安全距离）。最终，综合代价最低的路径将被选择用于无人机的下一步飞行。

1. 遍历所有候选路径：遍历所有可行的候选路径，以确保规划器不会遗漏任何潜在的最优路径。这些候选路径是基于先前生成的组合直方图和规划器的动态调整策略计算出来的。

2. 计算每条路径的代价：对每一条候选路径进行代价计算，代价综合考虑了与目标的距离、路径的平滑度和与障碍物的距离等因素。代价的计算结果将用于评估每条路径的优劣。

3. 选择代价最低的路径：通过比较所有候选路径的代价，最终选择代价最低的路径作为无人机的飞行路径。这条路径不仅要满足尽可能快到达目标的需求，还需要保持较高的安全性和良好的路径平滑度。

1. 检测障碍物距离：首先，系统会持续监测无人机与障碍物之间的距离，判断当前的飞行路径是否存在潜在的碰撞风险。这一步骤通过深度相机或激光雷达获取的点云数据来实现，以确保实时性和准确性。

2. 降低速度：当检测到与障碍物的距离小于设定的安全阈值时，规划器会指示无人机降低飞行速度，以避免发生碰撞。减速操作旨在给系统足够的时间来重新计算路径或执行紧急规避动作。

3. 保持当前速度：如果无人机与障碍物的距离在安全阈值之上，系统会保持当前的飞行速度，从而以最高效的方式到达目标点。保持速度有助于提升飞行的效率，减少不必要的延迟。

1. 根据选定路径调整姿态：在无人机执行避障机动时，首先需要根据选定的最优路径调整姿态。具体而言，无人机会调整其俯仰角、偏航角和滚转角，以确保无人机能够沿着规划好的路径前进。这些姿态调整是通过飞行控制系统来执行的，控制系统接收路径规划器的输出，计算相应的姿态调整指令。调整姿态的目标是使无人机的航向与预定路径一致，避免因路径偏差导致的碰撞风险。在这一过程中，姿态调整需要保持平稳，以避免因姿态变化过大引起的飞行不稳定或动力损耗。

2. 执行避障动作：在姿态调整完成之后，无人机会执行具体的避障动作，以确保安全通过障碍物区域。避障动作包括调整飞行速度、改变高度、侧向移动等操作，以避开路径中的障碍物。根据传感器的实时反馈，系统会评估当前路径上的障碍物距离和位置，并采取相应的避障策略。例如，当障碍物非常接近时，无人机会采取紧急规避措施，如迅速爬升、急速减速或改变航向，以避免与障碍物发生碰撞。在整个避障过程中，规划器和控制系统相互配合，通过不断更新的环境感知数据，动态调整避障策略，从而确保无人机在复杂动态环境中的飞行安全。

4.1.4 全局路径规划详细设计

1. 接收GPS数据: GPS数据提供无人机在全局环境中的绝对位置信息，是确定无人机相对目标点位置的关键。

2. 接收相机数据: 相机数据为局部环境的视觉信息，帮助检测障碍物和地形特征。

3. 融合传感器数据: 将GPS和相机数据融合，可以获得更加全面的环境信息，以便进行精确的全局路径规划。

1. 接收新传感器数据: 从相机、激光雷达等传感器接收最新的环境信息，确保地图能够反映实时变化。

2. 更新八叉树地图: 使用八叉树数据结构将环境信息进行空间划分，生成的地图能够高效表示复杂的三维环境，有助于快速查询和更新障碍物的位置。

1. 将三维空间划分为八叉树: 使用八叉树结构将三维空间逐层划分为八个子空间，每个子空间进一步划分，直到达到预设的精度。这种划分方式可以高效地管理空间中的障碍物信息。

2. 建立障碍物表示: 在八叉树中记录障碍物的状态，包括障碍物的空间位置和大小，帮助路径规划器快速判断当前路径的可行性。

1. 初始化开放列表和关闭列表: 开放列表用于记录待评估的节点，关闭列表用于记录已评估的节点。初始化这些列表是A*算法的基础步骤，确保从起点开始探索最优路径。

2. 评估节点代价: 对每个节点计算总代价，包括从起点到该节点的实际代价和从该节点到目标点的估计代价，确保选择最佳的路径。

3. 选择最优节点: 从开放列表中选择总代价最低的节点进行扩展，确保路径的高效性。

4. 生成路径: 当找到目标节点后，通过回溯生成从起点到目标点的完整路径，用于无人机的导航控制。

1. 监测地图变化: 实时监测八叉树地图，确保无人机能够感知环境的动态变化，如出现新的障碍物或已有障碍物的移动。

2. 重新规划路径: 判断环境中是否出现新障碍物或已有障碍物发生位置变化，如果出现新的障碍物，重新应用A*算法生成新的最优路径，以确保飞行安全。

3. 保持当前路径: 如果环境无变化，则继续沿当前路径飞行，保证飞行的连续性和稳定性。

1. 根据最优路径设定航路点: 根据A*算法生成的最优路径，设定一系列航路点，使无人机能够逐步飞向目标位置。

2. 输出到飞控系统: 将这些航路点发送给飞控系统，飞控系统根据这些点控制无人机的姿态和速度，确保飞行过程中的安全性和准确性。

4.2 技术关键点设计

技术关键点设计

4.2.1 3D VFH 算法

1. 直方图生成：根据点云数据，计算每个点相对于无人机位置的俯仰角和航向角，并将这些信息存储在直方图中。直方图的每个单元格表示某一特定方向的障碍物密度信息。

2. 代价函数计算：每个候选方向的代价通过代价函数进行评估。代价函数由多个部分组成，包括目标代价、平滑度代价等。目标代价衡量无人机在候选方向与目标方向之间的偏差，而平滑度代价衡量当前方向与上一时刻方向的差异。
目标代价函数由下式给出：
其中：
• ：目标代价，衡量候选方向与目标方向之间的偏差。
• ：候选方向 与目标位置 之间的偏航角差异。
• ：候选方向与目标位置之间的上升俯仰角差异。
• ：候选方向与目标位置之间的下降俯仰角差异。
• 、：上升和下降的权重因子，控制上升与下降方向的优先级。
平滑度代价函数由下式给出：
其中：
• ：平滑度代价，用于衡量当前候选方向与之前选择方向之间的角度变化。
• ：前一方向 和当前候选方向 之间的偏航角差异。
• ：前一方向与当前候选方向之间的俯仰角差异。
综合代价函数由下式给出：
其中：
• ：总代价，表示选择某路径的综合评估值。
• 、：目标和路径平滑性的权重因子，决定各个代价项的相对重要性。

3. 路径选择：通过启发式搜索，选择代价最小的路径作为无人机的运动方向。如果某个方向的代价最低且满足避障条件，无人机会沿着该方向移动。

4. 3D 投影与直方图构建：在每个时间步长，3D VFH 算法将上一时刻的直方图数据重新投影为三维点云，并将其与当前的点云数据结合，构建新的直方图。

4.2.2 安全措施与边界条件的调整

1. 限制移动方向在视场范围内：当存在多个代价相近的候选方向时，算法可能会频繁在这几个方向之间切换，导致无人机逐渐靠近障碍物并增加碰撞风险。因此，3D VFH 算法在移动方向选择上做了改进，要求下一步航路点必须位于无人机的当前视场（FOV）范围内。如果航路点不在视场范围内，无人机将先悬停并调整偏航，直到航路点进入视场范围 。

2. 安全裕度的设置：在直方图中，为每个被占用的单元格增加了安全裕度。这些安全裕度会扩大障碍物的影响范围，从而增加无人机与障碍物之间的最小距离。如果安全裕度设置过大，可能会导致无人机无法通过障碍物之间的狭窄通道；如果安全裕度设置过小，则增加了碰撞的风险。为了在接近障碍物时优先考虑安全，算法实现了两种不同大小的安全裕度，具体选择取决于最近障碍物的距离。

3. 代价参数的自适应调整：为了监控无人机朝向目标的进展，算法中引入了一个自适应的代价参数 ，用于控制无人机的上升代价。当无人机没有朝向目标的进展时，算法会降低上升代价，鼓励无人机改变高度以绕过障碍物：
进展速率公式由下式给出：
其中：
• ：在时间步长 处的进展速率。
• 、：分别是时间步长 和 时刻无人机到目标的距离。
• 、：分别是时间步长 和 时刻的时间。
权重因子的调整公式由下式给出：
其中：
• ：时间步长 处的上升代价因子。
• ：进展速率的临界值，判断当前进展是否足够。
• 、：分别是上升代价因子的增量和减量。
• 、：上升代价因子的最大和最小值。

3. 球体避障与最小距离约束

1. 球体约束的定义：球体避障约束通过在障碍物中心点 周围绘制一个半径为 的球体，确保无人机与障碍物之间保持最小距离。障碍物中心点由所有在半径 范围内的点云平均位置确定。

2. 航路点调整：在计算完航路点之后，球体避障模块会检查该航路点是否位于球体内部。如果航路点位于球体内部，算法会调整其高度使其接近球体的赤道位置，并将其投影到球体表面。
最终的航路点 根据下述公式确定：
其中：
• ：最终输出的航路点。
• ：投影到球体表面的航路点。
• 、：分别为临时航路点和原始航路点。
• ：球体中心位置，即障碍物中心点。
• ：球体半径，即无人机与障碍物之间的最小安全距离。
• ：迟滞因子，用于扩大球体影响范围。
• ：加权因子，用于计算航路点在球体范围内的偏移位置。
这种航路点的调整确保了无人机能够在障碍物周围飞行，而不是直接飞过或飞到障碍物下方 。

3. 迟滞机制：为了防止频繁切换路径，球体避障模块还引入了迟滞机制。通过扩大球体半径 ，在球体的影响范围内对航路点进行调整，以确保飞行的稳定性。

4. 建立记忆机制的 3DVFH*

1. 直方图的再投影：在每个时间步长中，记忆机制会将前一时刻的直方图数据投影为三维点云。由于这些点的角度信息依赖于无人机的当前位置，因此需要将这些点重新投影到当前的三维空间中。

2. 记忆直方图的构建：通过将再投影的点云数据构建为一个记忆直方图，并以较低的分辨率进行存储。随后，这个低分辨率的记忆直方图会被上采样至与当前直方图相同的分辨率。

3. 直方图的融合：最终直方图是通过融合记忆直方图和当前直方图得到的。对于无人机视场（FOV）范围内的区域，优先使用当前的直方图数据；对于视场外的区域，则通过逻辑 OR 操作融合记忆直方图和当前直方图的数据。
记忆机制使用如下公式将记忆直方图与新直方图结合：
其中：
• ：最终的直方图，用于当前时刻的导航。
• ：记忆直方图，由前一时刻的直方图投影到当前三维空间得到。
• ：新直方图，由当前时刻的传感器数据生成。

5. 地面检测与高度控制

1. 内部高度图的构建：无人机通过前视摄像头检测地面的高度信息，并将其存储在一个内部高度图中。通过对点云数据进行裁剪和 RANSAC 算法的平面拟合，可以得到地面的高度信息，并将这些信息存储为矩形区域。
高度平面拟合公式由下式给出：
其中：
• ：点 到拟合平面的垂直距离。
• ：点 的三维坐标。
• ：拟合平面的方程参数。
最终平面点计算公式由下式给出：
其中：
• ：点 在拟合平面上的投影点。

2. 无障碍情况下的高度控制：当周围没有障碍物时，无人机会直接朝向目标飞行。此时，高度控制模块会根据内部高度图中的信息来调整飞行高度，确保无人机能够提前上升，避免碰撞地面。
高度调整公式由下式给出：
其中：
• ：无人机在进入特定地面 patch 前的裕度。
• ：无人机当前位置的高度。
• ：目标地面 patch 的高度。
• ：无人机的最小飞行高度。
• ：最大上升角。

3. 结合直方图的高度控制：在存在障碍物的情况下，高度控制模块会与 3D VFH 算法相结合，通过对路径的上下调整，确保无人机既能够避开障碍物，又能保持安全的飞行高度。

5 工程实现

5.1 局部路径规划实现

5.2 全局路径规划实现

6 功能性能指标符合性分析

附录1：伴侣计算机MAVROS消息表

7.1 通信数据流图

7.2 通信描述

1. 节点：
• mavros_node：主通信节点。
• gsc_bridge：额外的代理。旧名称为ros_udp。
• event_launcher：监听解锁状态，触发并运行事件程序。配置示例。

2. 连接：通过URL定义连接，可以为FCU和GCS使用任何支持的类型。
• 协议：串口/带硬件流控的串口/UDP/UDP广播/TCP客户端/TCP服务器

3. 插件：由mavros_node加载的标准通信插件集。
• 3dr_radio：将3DR无线电状态发布到诊断和主题。
• actuator_control：向FCU发送执行器命令。
• hil_control：发布HIL_CONTROLS。
• command：向FCU发送COMMAND_LONG。
• ftp：支持MAVLink-FTP的插件（PX4）。
• global_position：发布由FCU融合的全球位置信息和原始GPS数据。
• imu_pub：发布IMU状态
• local_position：在TF和PoseStamped主题中发布LOCAL_POSITION_NED。
• manual_control：发布MANUAL_CONTROL消息（用户输入）。
• param：允许访问FCU参数并将其映射到~param/中的ROS参数。
• rc_io：发布RC接收器输入。
• safety_area：向FCU发送安全允许区域。初始区域可以从参数加载。
• setpoint_accel：发送加速度设定点。
• setpoint_position：使用SET_POSITION_TARGET_GLOBAL_INT或SET_POSITION_TARGET_LOCAL_NED发送位置设定点。
• setpoint_raw：向FCU发送原始设定点消息并提供回环主题（PX4）。
• setpoint_velocity：向FCU发送速度设定点。
• sys_status：系统状态插件。处理FCU检测。必需，永远不要将其列入黑名单！
• sys_time：系统时间插件。在PX4上进行时间同步。
• vfr_hud：发布VFR_HUD和WIND消息。
• waypoint：允许访问FCU任务（航点）。

附录2：飞行控制器模式设置

1. 基础飞行动作/ 飞行模式
上锁/ 锁定/ 起飞和降落/ 返航等
arming Services 加解锁服务
模式切换
PX4支持的模式有：

MODE

MANUAL

ACRO

ALTCTL

POSCTL

OFFBOARD

STABILIZED

RATTITUDE

AUTO.MISSION

AUTO.LOITER

AUTO.RTL

AUTO.LAND

AUTO.READY

AUTO.TAKEOFF

AUTO.FOLLOW_TARGET

AUTO.PRECLAND

飞行模式

手动

特技

定高

定点

外部控制

自稳

姿态特技

自动任务

自动留待

自动返航

自动降落

自动模式就绪

自动起飞

自动跟踪目标

精准着陆

是否需要定点信息

N

N

N

Y

Y

N

N

Y

Y

Y

Y

Y

Y

Y

Y

支持的飞控类型

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

ㅤ

参考链接：Flight Modes | PX4 User Guide
参考链接：Flight Modes — Copter documentation (ardupilot.org)

2. 参考坐标系
FRD机体坐标系：X Forward, Y Right, Z Down
NED坐标系：X North, Y East, Z Down
FLU机体坐标系：X Forward, Y Left, Z Up
The local/world and world frames used by ROS and PX4 are different.

FRAME	PX4	ROS
Body	FRD (X Forward, Y Right, Z Down)	FLU (X Forward, Y Left, Z Up), usually named base_link
World	FRD or NED (X North, Y East, Z Down)	FLU or ENU (X East, Y North, Z Up), with the naming being odom or map

3. 飞行控制功能
（全局/ 本地）位置控制/ 速度控制
actuator_control 控制飞控IO输出（mixing_and_actuators 混控器）
setpoint_accel 控制期望的加速度
setpoint_attitude 控制期望的姿态
setpoint_position 控制期望的位置（相对坐标或GPS坐标）
setpoint_velocity 控制期望的速度

4. 飞行数据获取
global_position 订阅GPS数据
imu_pub 订阅IMU信息
local_position 订阅本地位置数据
manual_control 订阅遥控器的值
sys_status 查询系统状态
waypoint 航点信息

5. 定位数据来源
通过修改EKF2位置融合数据获取来源来实现（EKF2_AID_MASK）
视觉数据融合有以下几种方式：
vision position fusion 使用视觉定位数据融合
vision velocity fusion 使用视觉速度数据融合
vision yaw fusion 使用视觉偏航数据融合
external vision rotation 使用外部视觉的旋转数据代替

6. 高度数据获取
通过修改EKF2高度融合数据获取来源来实现（EKF2_HGT_REF）
barometer 气压计高度
GNSS GPS高度
range sensor 测距传感器高度
vision 视觉高度