前言
无人机编队表演、农业植保多机协同、物流配送集群...这些应用的背后是一套复杂的技术体系：
- 编队控制 - 多架无人机如何保持队形？
- 协同通信 - 无人机之间如何交换信息？
- 防撞机制 - 如何防止相互碰撞？
- 电子围栏 - 如何限制飞行区域？
- 智能避障 - 深度学习如何实现实时避障？
今天从原理到代码，全面解析这套技术栈。
系统整体架构
┌─────────────────────────────────────────────────────────────────────────────┐
│ 无人机编队协同系统架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────────────┐ │
│ │ 地面站 (GCS) │ │
│ │ │ │
│ │ - 任务规划 │ │
│ │ - 编队管理 │ │
│ │ - 监控告警 │ │
│ │ - 电子围栏配置 │ │
│ └──────────┬──────────┘ │
│ │ │
│ ┌───────────────┼───────────────┐ │
│ │ 数据链通信 (2.4G/5.8G/4G) │ │
│ └───────────────┼───────────────┘ │
│ │ │
│ ┌──────────────────────────┼──────────────────────────┐ │
│ │ │ │ │
│ ▼ ▼ ▼ │
│ ┌───────────┐ ┌───────────┐ ┌───────────┐ │
│ │ 无人机1 │◄────────────►│ 无人机2 │◄────────────►│ 无人机3 │ │
│ │ (领航机) │ 机间通信 │ (跟随机) │ 机间通信 │ (跟随机) │ │
│ └─────┬─────┘ └─────┬─────┘ └─────┬─────┘ │
│ │ │ │ │
│ └──────────────────────────┼──────────────────────────┘ │
│ │ │
│ 自组织网络 │
│ (Ad-hoc / Mesh) │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 单机系统架构: │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ 无人机单机 │ │
│ │ │ │
│ │ ┌──────────────────────────────────────────────────────────────┐ │ │
│ │ │ 飞控系统 (FCU) │ │ │
│ │ │ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐ │ │ │
│ │ │ │ 姿态控制 │ │ 位置控制 │ │ 编队控制 │ │ 避障控制 │ │ │ │
│ │ │ └─────────┘ └─────────┘ └─────────┘ └─────────┘ │ │ │
│ │ └──────────────────────────────────────────────────────────────┘ │ │
│ │ ▲ │ │
│ │ ┌───────────────────┼───────────────────┐ │ │
│ │ │ │ │ │ │
│ │ ┌───────┴───────┐ ┌──────┴──────┐ ┌───────┴───────┐ │ │
│ │ │ 定位模块 │ │ 感知模块 │ │ 通信模块 │ │ │
│ │ │ GPS/RTK/UWB │ │ 视觉/雷达/ │ │ 数据链/WiFi │ │ │
│ │ │ │ │ 超声波 │ │ /4G/5G │ │ │
│ │ └───────────────┘ └─────────────┘ └───────────────┘ │ │
│ │ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────────┘编队控制算法
编队控制方法分类
┌─────────────────────────────────────────────────────────────────────────────┐
│ 编队控制方法对比 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【1. 领航-跟随法 (Leader-Follower)】 │
│ ───────────────────────────────────────── │
│ │
│ 领航机 ●────────────────────────────► │
│ ╲ │
│ ╲ 距离d, 角度θ │
│ ╲ │
│ 跟随机1 ○─────────────────────────► │
│ ╲ │
│ ╲ │
│ 跟随机2 ○────────────────────────► │
│ │
│ 优点: 结构简单，易实现 │
│ 缺点: 依赖领航机，单点故障 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【2. 虚拟结构法 (Virtual Structure)】 │
│ ───────────────────────────────────────── │
│ │
│ 虚拟刚体结构 │
│ ┌─────────────────────┐ │
│ │ ● ● ● │ ───────────► │
│ │ │ │
│ │ ● ● │ 整体移动 │
│ └─────────────────────┘ │
│ │
│ 所有无人机视为刚体上的点，保持相对位置不变 │
│ │
│ 优点: 队形保持好，协调性强 │
│ 缺点: 灵活性差，计算复杂 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【3. 行为法 (Behavior-based)】 │
│ ───────────────────────────────────────── │
│ │
│ 目标吸引 │
│ ↑ │
│ 障碍排斥 ← ● → 邻居跟随 │
│ ↓ │
│ 速度匹配 │
│ │
│ 多种行为加权融合: v = w1*v_target + w2*v_avoid + w3*v_follow + ... │
│ │
│ 优点: 适应性强，鲁棒性好 │
│ 缺点: 参数调节困难，队形不稳定 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【4. 一致性算法 (Consensus)】 │
│ ───────────────────────────────────────── │
│ │
│ ●──────● │
│ │╲ ╱│ │
│ │ ╲ ╱ │ 每个节点根据邻居状态更新自己 │
│ │ ╲╱ │ 最终达到一致 │
│ ●──────● │
│ │
│ ẋᵢ = -∑ aᵢⱼ(xᵢ - xⱼ) (一阶一致性) │
│ │
│ 优点: 分布式，无单点故障 │
│ 缺点: 收敛速度依赖网络拓扑 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘领航-跟随法实现
c
/**
* 领航-跟随编队控制
*
* 跟随机保持与领航机的期望距离d和方位角θ
*/
#include <stdio.h>
#include <math.h>
#include <string.h>
// 无人机状态
typedef struct {
uint8_t id;
// 位置 (NED坐标系，单位:米)
double x; // 北向
double y; // 东向
double z; // 下向 (高度取负)
// 速度
double vx;
double vy;
double vz;
// 姿态 (弧度)
double roll;
double pitch;
double yaw;
// 角速度
double roll_rate;
double pitch_rate;
double yaw_rate;
} UAVState_t;
// 编队参数
typedef struct {
double desired_distance; // 期望距离 (米)
double desired_angle; // 期望方位角 (弧度，相对于领航机航向)
double desired_altitude_offset; // 高度偏移 (米)
} FormationParam_t;
// 控制输出
typedef struct {
double vx_cmd; // 北向速度指令
double vy_cmd; // 东向速度指令
double vz_cmd; // 垂向速度指令
double yaw_cmd; // 航向指令
} ControlOutput_t;
// PID控制器
typedef struct {
double kp;
double ki;
double kd;
double integral;
double prev_error;
double integral_limit;
} PIDController_t;
/**
* PID控制器更新
*/
double pid_update(PIDController_t *pid, double error, double dt)
{
// 积分
pid->integral += error * dt;
// 积分限幅
if (pid->integral > pid->integral_limit) {
pid->integral = pid->integral_limit;
} else if (pid->integral < -pid->integral_limit) {
pid->integral = -pid->integral_limit;
}
// 微分
double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error;
// PID输出
return pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
}
/**
* 计算跟随机的期望位置
*
* 期望位置 = 领航机位置 + 旋转后的偏移向量
*/
void calculate_desired_position(const UAVState_t *leader,
const FormationParam_t *param,
double *desired_x, double *desired_y, double *desired_z)
{
// 在领航机机体坐标系下的偏移
// 距离d，方位角θ (相对于领航机航向)
double offset_body_x = param->desired_distance * cos(param->desired_angle);
double offset_body_y = param->desired_distance * sin(param->desired_angle);
// 旋转到NED坐标系 (使用领航机航向)
double cos_yaw = cos(leader->yaw);
double sin_yaw = sin(leader->yaw);
double offset_ned_x = offset_body_x * cos_yaw - offset_body_y * sin_yaw;
double offset_ned_y = offset_body_x * sin_yaw + offset_body_y * cos_yaw;
// 期望位置
*desired_x = leader->x + offset_ned_x;
*desired_y = leader->y + offset_ned_y;
*desired_z = leader->z + param->desired_altitude_offset;
}
// 编队控制器
typedef struct {
PIDController_t pid_x;
PIDController_t pid_y;
PIDController_t pid_z;
FormationParam_t param;
} FormationController_t;
/**
* 初始化编队控制器
*/
void formation_controller_init(FormationController_t *ctrl,
double distance, double angle, double alt_offset)
{
// 位置PID参数
ctrl->pid_x.kp = 1.0;
ctrl->pid_x.ki = 0.1;
ctrl->pid_x.kd = 0.5;
ctrl->pid_x.integral = 0;
ctrl->pid_x.prev_error = 0;
ctrl->pid_x.integral_limit = 5.0;
ctrl->pid_y = ctrl->pid_x;
ctrl->pid_z = ctrl->pid_x;
ctrl->pid_z.kp = 1.5; // 高度控制更激进
// 编队参数
ctrl->param.desired_distance = distance;
ctrl->param.desired_angle = angle;
ctrl->param.desired_altitude_offset = alt_offset;
}
/**
* 编队控制更新
*/
void formation_control_update(FormationController_t *ctrl,
const UAVState_t *leader,
const UAVState_t *follower,
double dt,
ControlOutput_t *output)
{
// 计算期望位置
double desired_x, desired_y, desired_z;
calculate_desired_position(leader, &ctrl->param,
&desired_x, &desired_y, &desired_z);
// 位置误差
double error_x = desired_x - follower->x;
double error_y = desired_y - follower->y;
double error_z = desired_z - follower->z;
// PID控制
output->vx_cmd = pid_update(&ctrl->pid_x, error_x, dt);
output->vy_cmd = pid_update(&ctrl->pid_y, error_y, dt);
output->vz_cmd = pid_update(&ctrl->pid_z, error_z, dt);
// 速度限幅
double max_vel = 5.0; // 最大速度 5m/s
if (output->vx_cmd > max_vel) output->vx_cmd = max_vel;
if (output->vx_cmd < -max_vel) output->vx_cmd = -max_vel;
if (output->vy_cmd > max_vel) output->vy_cmd = max_vel;
if (output->vy_cmd < -max_vel) output->vy_cmd = -max_vel;
if (output->vz_cmd > max_vel) output->vz_cmd = max_vel;
if (output->vz_cmd < -max_vel) output->vz_cmd = -max_vel;
// 航向跟随领航机
output->yaw_cmd = leader->yaw;
}
/**
* 多机编队管理器
*/
#define MAX_UAVS 20
typedef struct {
UAVState_t uavs[MAX_UAVS];
int uav_count;
int leader_id;
FormationController_t controllers[MAX_UAVS];
// 编队类型
enum {
FORMATION_LINE, // 一字形
FORMATION_V, // V字形
FORMATION_DIAMOND, // 菱形
FORMATION_CIRCLE, // 圆形
} formation_type;
double formation_spacing; // 编队间距
} FormationManager_t;
/**
* 初始化编队
*/
void formation_manager_init(FormationManager_t *mgr, int formation_type,
double spacing, int leader_id)
{
mgr->formation_type = formation_type;
mgr->formation_spacing = spacing;
mgr->leader_id = leader_id;
// 根据编队类型计算每架飞机的相对位置
for (int i = 0; i < mgr->uav_count; i++) {
if (mgr->uavs[i].id == leader_id) continue;
double distance, angle, alt_offset;
int follower_index = (i < leader_id) ? i : i - 1;
switch (formation_type) {
case FORMATION_LINE:
// 一字形: 所有跟随机在领航机后方
distance = spacing * (follower_index + 1);
angle = M_PI; // 正后方
alt_offset = 0;
break;
case FORMATION_V:
// V字形: 交替左右后方
distance = spacing * ((follower_index / 2) + 1) * sqrt(2);
angle = (follower_index % 2 == 0) ?
(M_PI - M_PI/4) : (M_PI + M_PI/4); // 左后/右后45度
alt_offset = 0;
break;
case FORMATION_DIAMOND:
// 菱形
if (follower_index == 0) {
distance = spacing;
angle = M_PI; // 正后方
} else if (follower_index == 1) {
distance = spacing;
angle = M_PI/2; // 正右方
} else if (follower_index == 2) {
distance = spacing;
angle = -M_PI/2; // 正左方
} else {
distance = spacing * 2;
angle = M_PI;
}
alt_offset = 0;
break;
case FORMATION_CIRCLE:
// 圆形: 等间距分布
{
int total_followers = mgr->uav_count - 1;
double angle_step = 2 * M_PI / total_followers;
distance = spacing;
angle = angle_step * follower_index;
alt_offset = 0;
}
break;
}
formation_controller_init(&mgr->controllers[i], distance, angle, alt_offset);
}
}
/**
* 更新编队控制
*/
void formation_manager_update(FormationManager_t *mgr, double dt)
{
// 获取领航机状态
UAVState_t *leader = NULL;
for (int i = 0; i < mgr->uav_count; i++) {
if (mgr->uavs[i].id == mgr->leader_id) {
leader = &mgr->uavs[i];
break;
}
}
if (leader == NULL) return;
// 更新每架跟随机
for (int i = 0; i < mgr->uav_count; i++) {
if (mgr->uavs[i].id == mgr->leader_id) continue;
ControlOutput_t output;
formation_control_update(&mgr->controllers[i], leader, &mgr->uavs[i],
dt, &output);
// 将控制指令发送给飞控
// send_velocity_command(mgr->uavs[i].id, &output);
}
}一致性算法实现
c
/**
* 分布式一致性编队控制
*
* 无需领航机，所有节点地位平等
* 通过邻居信息交换达到一致
*/
// 通信邻居矩阵 (邻接矩阵)
typedef struct {
uint8_t adj_matrix[MAX_UAVS][MAX_UAVS]; // 1表示可通信
double weights[MAX_UAVS][MAX_UAVS]; // 通信权重
int node_count;
} CommunicationTopology_t;
// 期望编队偏移 (相对于编队中心)
typedef struct {
double offset_x[MAX_UAVS];
double offset_y[MAX_UAVS];
double offset_z[MAX_UAVS];
} FormationOffset_t;
// 一致性控制器
typedef struct {
CommunicationTopology_t topology;
FormationOffset_t offsets;
double consensus_gain; // 一致性增益
double damping; // 阻尼系数
double virtual_target_gain; // 虚拟目标增益
} ConsensusController_t;
/**
* 初始化通信拓扑
*/
void init_topology(CommunicationTopology_t *topo, int node_count)
{
topo->node_count = node_count;
// 默认全连接
for (int i = 0; i < node_count; i++) {
for (int j = 0; j < node_count; j++) {
if (i != j) {
topo->adj_matrix[i][j] = 1;
topo->weights[i][j] = 1.0 / (node_count - 1);
} else {
topo->adj_matrix[i][j] = 0;
topo->weights[i][j] = 0;
}
}
}
}
/**
* 一致性协议更新
*
* 状态更新方程:
* ẋᵢ = -α Σⱼ aᵢⱼ [(xᵢ - δᵢ) - (xⱼ - δⱼ)] + β(x_target - xᵢ) - γẋᵢ
*
* 其中:
* - α: 一致性增益
* - aᵢⱼ: 邻接权重
* - δ: 期望编队偏移
* - β: 虚拟目标增益
* - γ: 阻尼系数
*/
void consensus_update(ConsensusController_t *ctrl,
UAVState_t *uavs, int uav_count,
double target_x, double target_y, double target_z,
double dt,
ControlOutput_t *outputs)
{
double alpha = ctrl->consensus_gain;
double beta = ctrl->virtual_target_gain;
double gamma = ctrl->damping;
for (int i = 0; i < uav_count; i++) {
double ax = 0, ay = 0, az = 0;
// 一致性项: 与邻居对齐
for (int j = 0; j < uav_count; j++) {
if (ctrl->topology.adj_matrix[i][j]) {
double wij = ctrl->topology.weights[i][j];
// 考虑编队偏移的相对位置
double rel_xi = uavs[i].x - ctrl->offsets.offset_x[i];
double rel_yi = uavs[i].y - ctrl->offsets.offset_y[i];
double rel_zi = uavs[i].z - ctrl->offsets.offset_z[i];
double rel_xj = uavs[j].x - ctrl->offsets.offset_x[j];
double rel_yj = uavs[j].y - ctrl->offsets.offset_y[j];
double rel_zj = uavs[j].z - ctrl->offsets.offset_z[j];
ax -= alpha * wij * (rel_xi - rel_xj);
ay -= alpha * wij * (rel_yi - rel_yj);
az -= alpha * wij * (rel_zi - rel_zj);
}
}
// 目标吸引项
double desired_x = target_x + ctrl->offsets.offset_x[i];
double desired_y = target_y + ctrl->offsets.offset_y[i];
double desired_z = target_z + ctrl->offsets.offset_z[i];
ax += beta * (desired_x - uavs[i].x);
ay += beta * (desired_y - uavs[i].y);
az += beta * (desired_z - uavs[i].z);
// 阻尼项
ax -= gamma * uavs[i].vx;
ay -= gamma * uavs[i].vy;
az -= gamma * uavs[i].vz;
// 速度积分
outputs[i].vx_cmd = uavs[i].vx + ax * dt;
outputs[i].vy_cmd = uavs[i].vy + ay * dt;
outputs[i].vz_cmd = uavs[i].vz + az * dt;
// 限幅
double max_vel = 5.0;
double vel_mag = sqrt(outputs[i].vx_cmd * outputs[i].vx_cmd +
outputs[i].vy_cmd * outputs[i].vy_cmd +
outputs[i].vz_cmd * outputs[i].vz_cmd);
if (vel_mag > max_vel) {
outputs[i].vx_cmd *= max_vel / vel_mag;
outputs[i].vy_cmd *= max_vel / vel_mag;
outputs[i].vz_cmd *= max_vel / vel_mag;
}
}
}
/**
* 设置编队形状偏移
*/
void set_formation_shape(ConsensusController_t *ctrl, int formation_type,
double spacing, int uav_count)
{
// 计算编队中心
// 所有偏移相对于编队中心
switch (formation_type) {
case FORMATION_LINE:
// 一字形
for (int i = 0; i < uav_count; i++) {
ctrl->offsets.offset_x[i] = (i - uav_count / 2.0) * spacing;
ctrl->offsets.offset_y[i] = 0;
ctrl->offsets.offset_z[i] = 0;
}
break;
case FORMATION_V:
// V字形
ctrl->offsets.offset_x[0] = 0; // 中心点
ctrl->offsets.offset_y[0] = 0;
ctrl->offsets.offset_z[0] = 0;
for (int i = 1; i < uav_count; i++) {
int row = (i + 1) / 2;
int side = (i % 2 == 0) ? 1 : -1; // 左右交替
ctrl->offsets.offset_x[i] = -row * spacing * 0.707; // 后方
ctrl->offsets.offset_y[i] = side * row * spacing * 0.707;
ctrl->offsets.offset_z[i] = 0;
}
break;
case FORMATION_CIRCLE:
// 圆形
for (int i = 0; i < uav_count; i++) {
double angle = 2 * M_PI * i / uav_count;
ctrl->offsets.offset_x[i] = spacing * cos(angle);
ctrl->offsets.offset_y[i] = spacing * sin(angle);
ctrl->offsets.offset_z[i] = 0;
}
break;
}
}防撞机制
多层防撞架构
┌─────────────────────────────────────────────────────────────────────────────┐
│ 多层防撞架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【第1层: 协同规划层】 - 任务前 │
│ ───────────────────────────────────────── │
│ │
│ - 4D轨迹规划 (x, y, z, t) │
│ - 时空冲突检测 │
│ - 全局路径优化 │
│ │
│ 在任务开始前确保各架飞机的轨迹不相交 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【第2层: 编队协调层】 - 运行时远距离 │
│ ───────────────────────────────────────── │
│ │
│ 检测范围: 50-200米 │
│ 数据源: GPS广播、ADS-B、通信交换 │
│ │
│ ● │
│ ╱│╲ │
│ ╱ │ ╲ 安全距离 │
│ ╱ │ ╲ 监控区 │
│ ╱ │ ╲ │
│ ○────┼────○ │
│ │ │
│ ○ │
│ │
│ 行为: 调整编队参数，提前规避 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【第3层: 感知避障层】 - 近距离 │
│ ───────────────────────────────────────── │
│ │
│ 检测范围: 5-50米 │
│ 数据源: 雷达、视觉、超声波 │
│ │
│ ┌───────────────────┐ │
│ │ 避障传感器 │ │
│ │ 检测到障碍物 │ │
│ └─────────┬─────────┘ │
│ │ │
│ ▼ │
│ ┌───────────────────┐ │
│ │ 路径重规划 │ │
│ │ 生成避障轨迹 │ │
│ └─────────┬─────────┘ │
│ │ │
│ ▼ │
│ ┌───────────────────┐ │
│ │ 跟踪控制器 │ │
│ │ 执行避障 │ │
│ └───────────────────┘ │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【第4层: 紧急避撞层】 - 极近距离 │
│ ───────────────────────────────────────── │
│ │
│ 检测范围: <5米 │
│ 触发条件: TCAS告警 │
│ │
│ ！！！ 紧急 ！！！ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────┐ │
│ │ 紧急机动指令 │ │
│ │ - 垂直分离 (一升一降) │ │
│ │ - 水平分离 (一左一右) │ │
│ │ - 紧急悬停 │ │
│ └─────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────────┘防撞算法实现
c
/**
* 多层防撞系统实现
*/
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
// 碰撞威胁等级
typedef enum {
THREAT_NONE, // 无威胁
THREAT_MONITOR, // 监控 (50-100m)
THREAT_CAUTION, // 注意 (20-50m)
THREAT_WARNING, // 警告 (10-20m)
THREAT_CRITICAL, // 危急 (<10m)
} ThreatLevel_t;
// 安全距离配置
typedef struct {
double monitor_distance; // 监控距离
double caution_distance; // 注意距离
double warning_distance; // 警告距离
double critical_distance; // 危急距离
double min_separation; // 最小安全间隔
} SafetyConfig_t;
static SafetyConfig_t safety_config = {
.monitor_distance = 100.0,
.caution_distance = 50.0,
.warning_distance = 20.0,
.critical_distance = 10.0,
.min_separation = 5.0,
};
// 碰撞检测结果
typedef struct {
bool collision_detected;
int threat_uav_id;
ThreatLevel_t threat_level;
double distance;
double closing_speed; // 接近速度 (正值表示接近)
double time_to_collision; // 碰撞剩余时间 (秒)
// 相对位置
double rel_x;
double rel_y;
double rel_z;
} CollisionInfo_t;
/**
* 计算两架无人机之间的距离
*/
double calculate_distance(const UAVState_t *uav1, const UAVState_t *uav2)
{
double dx = uav1->x - uav2->x;
double dy = uav1->y - uav2->y;
double dz = uav1->z - uav2->z;
return sqrt(dx * dx + dy * dy + dz * dz);
}
/**
* 计算接近速度
*
* 正值表示两机正在接近
*/
double calculate_closing_speed(const UAVState_t *uav1, const UAVState_t *uav2)
{
// 相对位置向量
double dx = uav2->x - uav1->x;
double dy = uav2->y - uav1->y;
double dz = uav2->z - uav1->z;
double distance = sqrt(dx * dx + dy * dy + dz * dz);
if (distance < 0.01) return 0; // 防止除零
// 单位向量
double ux = dx / distance;
double uy = dy / distance;
double uz = dz / distance;
// 相对速度
double dvx = uav2->vx - uav1->vx;
double dvy = uav2->vy - uav1->vy;
double dvz = uav2->vz - uav1->vz;
// 接近速度 = 相对速度在连线方向的投影 (取反，正值表示接近)
return -(dvx * ux + dvy * uy + dvz * uz);
}
/**
* 评估碰撞威胁
*/
void assess_collision_threat(const UAVState_t *self, const UAVState_t *other,
CollisionInfo_t *info)
{
info->collision_detected = false;
info->threat_uav_id = other->id;
// 计算距离
info->distance = calculate_distance(self, other);
// 相对位置
info->rel_x = other->x - self->x;
info->rel_y = other->y - self->y;
info->rel_z = other->z - self->z;
// 计算接近速度
info->closing_speed = calculate_closing_speed(self, other);
// 计算碰撞剩余时间
if (info->closing_speed > 0.1) { // 正在接近
info->time_to_collision = info->distance / info->closing_speed;
} else {
info->time_to_collision = 9999; // 不会碰撞
}
// 评估威胁等级
if (info->distance < safety_config.critical_distance) {
info->threat_level = THREAT_CRITICAL;
info->collision_detected = true;
} else if (info->distance < safety_config.warning_distance) {
info->threat_level = THREAT_WARNING;
info->collision_detected = true;
} else if (info->distance < safety_config.caution_distance) {
info->threat_level = THREAT_CAUTION;
// 只有在接近时才标记碰撞检测
if (info->closing_speed > 0.5) {
info->collision_detected = true;
}
} else if (info->distance < safety_config.monitor_distance) {
info->threat_level = THREAT_MONITOR;
} else {
info->threat_level = THREAT_NONE;
}
// 基于碰撞时间的额外检查
if (info->time_to_collision < 5.0 && info->closing_speed > 1.0) {
if (info->threat_level < THREAT_WARNING) {
info->threat_level = THREAT_WARNING;
info->collision_detected = true;
}
}
if (info->time_to_collision < 2.0 && info->closing_speed > 1.0) {
info->threat_level = THREAT_CRITICAL;
info->collision_detected = true;
}
}
/**
* 生成避撞指令
*/
typedef struct {
double avoid_vx;
double avoid_vy;
double avoid_vz;
bool emergency_stop;
} AvoidanceCommand_t;
void generate_avoidance_command(const UAVState_t *self,
const CollisionInfo_t *threat,
AvoidanceCommand_t *cmd)
{
cmd->emergency_stop = false;
cmd->avoid_vx = 0;
cmd->avoid_vy = 0;
cmd->avoid_vz = 0;
if (!threat->collision_detected) {
return;
}
// 计算避让方向 (远离威胁源)
double distance = threat->distance;
if (distance < 0.1) distance = 0.1; // 防止除零
double avoid_dir_x = -threat->rel_x / distance;
double avoid_dir_y = -threat->rel_y / distance;
double avoid_dir_z = -threat->rel_z / distance;
// 根据威胁等级确定避让强度
double avoid_speed;
switch (threat->threat_level) {
case THREAT_CRITICAL:
// 紧急避撞
if (threat->time_to_collision < 1.0) {
cmd->emergency_stop = true;
avoid_speed = 0; // 紧急悬停
} else {
avoid_speed = 5.0; // 最大避让速度
}
// 垂直分离策略 (根据ID决定升降)
if (self->id % 2 == 0) {
cmd->avoid_vz = -3.0; // 上升
} else {
cmd->avoid_vz = 3.0; // 下降
}
break;
case THREAT_WARNING:
avoid_speed = 3.0;
break;
case THREAT_CAUTION:
avoid_speed = 1.5;
break;
default:
avoid_speed = 0;
break;
}
if (!cmd->emergency_stop) {
cmd->avoid_vx = avoid_dir_x * avoid_speed;
cmd->avoid_vy = avoid_dir_y * avoid_speed;
if (cmd->avoid_vz == 0) {
cmd->avoid_vz = avoid_dir_z * avoid_speed * 0.5; // 垂直避让减半
}
}
}
/**
* 人工势场法避障
*
* 斥力场: 障碍物产生斥力，越近斥力越大
* 引力场: 目标产生引力
* 合力场: 引力 + 斥力
*/
typedef struct {
double rep_gain; // 斥力增益
double rep_range; // 斥力作用范围
double att_gain; // 引力增益
} PotentialFieldConfig_t;
static PotentialFieldConfig_t pf_config = {
.rep_gain = 10.0,
.rep_range = 30.0,
.att_gain = 1.0,
};
/**
* 计算斥力 (远离障碍物)
*/
void calculate_repulsive_force(const UAVState_t *self,
const UAVState_t *obstacles, int obstacle_count,
double *fx, double *fy, double *fz)
{
*fx = 0;
*fy = 0;
*fz = 0;
for (int i = 0; i < obstacle_count; i++) {
double dx = self->x - obstacles[i].x;
double dy = self->y - obstacles[i].y;
double dz = self->z - obstacles[i].z;
double distance = sqrt(dx * dx + dy * dy + dz * dz);
if (distance < pf_config.rep_range && distance > 0.1) {
// 斥力公式: F_rep = k_rep * (1/d - 1/d0) * (1/d^2) * n
// 简化为: F_rep = k_rep / d^2 * n (在作用范围内)
double force_mag = pf_config.rep_gain / (distance * distance);
// 限幅
if (force_mag > 10.0) force_mag = 10.0;
// 方向: 远离障碍物
*fx += force_mag * dx / distance;
*fy += force_mag * dy / distance;
*fz += force_mag * dz / distance;
}
}
}
/**
* 计算引力 (朝向目标)
*/
void calculate_attractive_force(const UAVState_t *self,
double target_x, double target_y, double target_z,
double *fx, double *fy, double *fz)
{
double dx = target_x - self->x;
double dy = target_y - self->y;
double dz = target_z - self->z;
double distance = sqrt(dx * dx + dy * dy + dz * dz);
if (distance > 0.1) {
// 线性引力: F_att = k_att * d * n
// 限制最大引力
double force_mag = pf_config.att_gain * distance;
if (force_mag > 5.0) force_mag = 5.0;
*fx = force_mag * dx / distance;
*fy = force_mag * dy / distance;
*fz = force_mag * dz / distance;
} else {
*fx = 0;
*fy = 0;
*fz = 0;
}
}
/**
* 势场法避障控制
*/
void potential_field_control(const UAVState_t *self,
const UAVState_t *neighbors, int neighbor_count,
double target_x, double target_y, double target_z,
ControlOutput_t *output)
{
double rep_fx, rep_fy, rep_fz;
double att_fx, att_fy, att_fz;
// 计算斥力 (来自邻居)
calculate_repulsive_force(self, neighbors, neighbor_count,
&rep_fx, &rep_fy, &rep_fz);
// 计算引力 (来自目标)
calculate_attractive_force(self, target_x, target_y, target_z,
&att_fx, &att_fy, &att_fz);
// 合力
double total_fx = att_fx + rep_fx;
double total_fy = att_fy + rep_fy;
double total_fz = att_fz + rep_fz;
// 力转换为速度指令 (简化: 直接使用)
output->vx_cmd = total_fx;
output->vy_cmd = total_fy;
output->vz_cmd = total_fz;
// 速度限幅
double max_vel = 5.0;
double vel_mag = sqrt(output->vx_cmd * output->vx_cmd +
output->vy_cmd * output->vy_cmd +
output->vz_cmd * output->vz_cmd);
if (vel_mag > max_vel) {
output->vx_cmd *= max_vel / vel_mag;
output->vy_cmd *= max_vel / vel_mag;
output->vz_cmd *= max_vel / vel_mag;
}
}GPS定位与电子围栏
电子围栏系统
┌─────────────────────────────────────────────────────────────────────────────┐
│ 电子围栏系统 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【电子围栏类型】 │
│ ───────────────────────────────────────── │
│ │
│ 1. 包含围栏 (Include Fence) - 限制在区域内 │
│ ┌────────────────────────────────────┐ │
│ │ │ │
│ │ 允许飞行区域 │ │
│ │ │ │
│ │ ✈️ 无人机 │ │
│ │ │ │
│ └────────────────────────────────────┘ │
│ 超出边界 → 自动返回/悬停 │
│ │
│ 2. 排除围栏 (Exclude Fence) - 禁止进入区域 │
│ ┌─────────┐ │
│ │ 禁飞区 │ │
│ │ (机场) │ │
│ └─────────┘ │
│ 接近禁飞区 → 绕行/禁止进入 │
│ │
│ 3. 高度围栏 (Altitude Fence) │
│ │
│ ─────────────────────── 最大高度 120m │
│ │
│ ✈️ │
│ │
│ ─────────────────────── 最小高度 5m │
│ ═══════════════════════ 地面 │
│ │
│ 4. 组合围栏 │
│ ┌─────────────────────────────────────────────┐ │
│ │ 作业区域 (包含) │ │
│ │ ┌─────────┐ │ │
│ │ │ 建筑物 │ (排除) │ │
│ │ │ │ │ │
│ │ └─────────┘ │ │
│ │ │ │
│ │ ┌──────┐ │ │
│ │ │ 高压线│ (排除) │ │
│ │ └──────┘ │ │
│ └─────────────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────────┘电子围栏实现
c
/**
* 电子围栏系统实现
*/
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
// 地理坐标点
typedef struct {
double latitude; // 纬度 (度)
double longitude; // 经度 (度)
double altitude; // 高度 (米，相对于起飞点)
} GeoPoint_t;
// 围栏类型
typedef enum {
FENCE_TYPE_INCLUDE, // 包含围栏 (必须在内部)
FENCE_TYPE_EXCLUDE, // 排除围栏 (禁止进入)
FENCE_TYPE_ALTITUDE, // 高度围栏
} FenceType_t;
// 围栏形状
typedef enum {
FENCE_SHAPE_POLYGON, // 多边形
FENCE_SHAPE_CIRCLE, // 圆形
FENCE_SHAPE_CYLINDER, // 圆柱体 (圆形+高度)
} FenceShape_t;
// 围栏动作
typedef enum {
FENCE_ACTION_NONE, // 无动作 (仅告警)
FENCE_ACTION_WARN, // 警告
FENCE_ACTION_LOITER, // 悬停
FENCE_ACTION_RTL, // 返回起飞点
FENCE_ACTION_LAND, // 就地降落
FENCE_ACTION_BRAKE, // 刹车 (停止运动)
} FenceAction_t;
// 围栏定义
typedef struct {
uint8_t fence_id;
FenceType_t type;
FenceShape_t shape;
FenceAction_t action;
// 多边形围栏
GeoPoint_t polygon_points[32];
int polygon_point_count;
// 圆形围栏
GeoPoint_t center;
double radius; // 米
// 高度限制
double min_altitude; // 最小高度
double max_altitude; // 最大高度
// 缓冲区
double buffer_distance; // 预警缓冲区 (米)
bool enabled;
} GeofenceDefinition_t;
// 围栏管理器
#define MAX_FENCES 20
typedef struct {
GeofenceDefinition_t fences[MAX_FENCES];
int fence_count;
GeoPoint_t home_position; // 起飞点/返航点
// 当前违规状态
bool is_breaching;
uint8_t breaching_fence_id;
FenceAction_t current_action;
} GeofenceManager_t;
static GeofenceManager_t geofence_mgr;
// 地球半径 (米)
#define EARTH_RADIUS 6371000.0
/**
* 计算两个GPS点之间的距离 (Haversine公式)
*/
double gps_distance(const GeoPoint_t *p1, const GeoPoint_t *p2)
{
double lat1 = p1->latitude * M_PI / 180.0;
double lat2 = p2->latitude * M_PI / 180.0;
double dlat = (p2->latitude - p1->latitude) * M_PI / 180.0;
double dlon = (p2->longitude - p1->longitude) * M_PI / 180.0;
double a = sin(dlat / 2) * sin(dlat / 2) +
cos(lat1) * cos(lat2) * sin(dlon / 2) * sin(dlon / 2);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
return EARTH_RADIUS * c;
}
/**
* 检查点是否在多边形内 (射线法)
*/
bool point_in_polygon(const GeoPoint_t *point,
const GeoPoint_t *polygon, int vertex_count)
{
bool inside = false;
int j = vertex_count - 1;
for (int i = 0; i < vertex_count; i++) {
if (((polygon[i].latitude > point->latitude) !=
(polygon[j].latitude > point->latitude)) &&
(point->longitude < (polygon[j].longitude - polygon[i].longitude) *
(point->latitude - polygon[i].latitude) /
(polygon[j].latitude - polygon[i].latitude) + polygon[i].longitude)) {
inside = !inside;
}
j = i;
}
return inside;
}
/**
* 计算点到多边形边界的最近距离
*/
double distance_to_polygon_edge(const GeoPoint_t *point,
const GeoPoint_t *polygon, int vertex_count)
{
double min_distance = 1e10;
for (int i = 0; i < vertex_count; i++) {
int j = (i + 1) % vertex_count;
// 计算点到线段的距离
double d = gps_distance(point, &polygon[i]);
if (d < min_distance) min_distance = d;
d = gps_distance(point, &polygon[j]);
if (d < min_distance) min_distance = d;
// 这里简化处理，完整实现需要计算点到线段的垂直距离
}
return min_distance;
}
/**
* 检查单个围栏违规
*/
typedef struct {
bool is_violation; // 是否违规
bool is_approaching; // 是否接近边界
double distance_to_boundary;// 到边界的距离
FenceAction_t required_action;
} FenceCheckResult_t;
void check_fence(const GeofenceDefinition_t *fence,
const GeoPoint_t *position,
FenceCheckResult_t *result)
{
result->is_violation = false;
result->is_approaching = false;
result->distance_to_boundary = 0;
result->required_action = FENCE_ACTION_NONE;
if (!fence->enabled) {
return;
}
bool inside;
double distance;
switch (fence->shape) {
case FENCE_SHAPE_POLYGON:
inside = point_in_polygon(position, fence->polygon_points,
fence->polygon_point_count);
distance = distance_to_polygon_edge(position, fence->polygon_points,
fence->polygon_point_count);
break;
case FENCE_SHAPE_CIRCLE:
case FENCE_SHAPE_CYLINDER:
distance = gps_distance(position, &fence->center);
inside = (distance <= fence->radius);
distance = fence->radius - distance; // 到边界的距离
if (distance < 0) distance = -distance;
break;
}
// 检查高度
bool altitude_ok = (position->altitude >= fence->min_altitude &&
position->altitude <= fence->max_altitude);
// 判断违规
switch (fence->type) {
case FENCE_TYPE_INCLUDE:
// 包含围栏: 必须在内部
if (!inside || !altitude_ok) {
result->is_violation = true;
}
break;
case FENCE_TYPE_EXCLUDE:
// 排除围栏: 禁止在内部
if (inside) {
result->is_violation = true;
}
break;
case FENCE_TYPE_ALTITUDE:
if (!altitude_ok) {
result->is_violation = true;
if (position->altitude < fence->min_altitude) {
result->distance_to_boundary = fence->min_altitude - position->altitude;
} else {
result->distance_to_boundary = position->altitude - fence->max_altitude;
}
}
break;
}
result->distance_to_boundary = distance;
// 检查是否接近边界 (缓冲区)
if (!result->is_violation && distance < fence->buffer_distance) {
result->is_approaching = true;
}
// 设置动作
if (result->is_violation) {
result->required_action = fence->action;
} else if (result->is_approaching) {
result->required_action = FENCE_ACTION_WARN;
}
}
/**
* 检查所有围栏
*/
FenceCheckResult_t check_all_fences(const GeoPoint_t *position)
{
FenceCheckResult_t worst_result = {0};
worst_result.distance_to_boundary = 1e10;
for (int i = 0; i < geofence_mgr.fence_count; i++) {
FenceCheckResult_t result;
check_fence(&geofence_mgr.fences[i], position, &result);
// 取最严重的结果
if (result.is_violation) {
if (!worst_result.is_violation ||
result.required_action > worst_result.required_action) {
worst_result = result;
}
} else if (result.is_approaching && !worst_result.is_violation) {
if (result.distance_to_boundary < worst_result.distance_to_boundary) {
worst_result = result;
}
}
}
return worst_result;
}
/**
* 执行围栏动作
*/
void execute_fence_action(FenceAction_t action, const GeoPoint_t *current_pos)
{
switch (action) {
case FENCE_ACTION_NONE:
// 无动作
break;
case FENCE_ACTION_WARN:
// 发送警告
// send_warning("Approaching geofence boundary");
printf("WARNING: Approaching geofence boundary\n");
break;
case FENCE_ACTION_LOITER:
// 悬停
// set_flight_mode(MODE_LOITER);
printf("ACTION: Loiter - holding position\n");
break;
case FENCE_ACTION_RTL:
// 返航
// set_flight_mode(MODE_RTL);
printf("ACTION: Return to Launch\n");
break;
case FENCE_ACTION_LAND:
// 降落
// set_flight_mode(MODE_LAND);
printf("ACTION: Landing\n");
break;
case FENCE_ACTION_BRAKE:
// 刹车
// set_velocity(0, 0, 0);
printf("ACTION: Brake - stopping\n");
break;
}
}
/**
* 添加多边形围栏
*/
int add_polygon_fence(FenceType_t type, FenceAction_t action,
const GeoPoint_t *points, int point_count,
double min_alt, double max_alt, double buffer)
{
if (geofence_mgr.fence_count >= MAX_FENCES) {
return -1;
}
GeofenceDefinition_t *fence = &geofence_mgr.fences[geofence_mgr.fence_count];
fence->fence_id = geofence_mgr.fence_count;
fence->type = type;
fence->shape = FENCE_SHAPE_POLYGON;
fence->action = action;
fence->polygon_point_count = point_count;
for (int i = 0; i < point_count; i++) {
fence->polygon_points[i] = points[i];
}
fence->min_altitude = min_alt;
fence->max_altitude = max_alt;
fence->buffer_distance = buffer;
fence->enabled = true;
geofence_mgr.fence_count++;
return fence->fence_id;
}
/**
* 添加圆形围栏
*/
int add_circular_fence(FenceType_t type, FenceAction_t action,
const GeoPoint_t *center, double radius,
double min_alt, double max_alt, double buffer)
{
if (geofence_mgr.fence_count >= MAX_FENCES) {
return -1;
}
GeofenceDefinition_t *fence = &geofence_mgr.fences[geofence_mgr.fence_count];
fence->fence_id = geofence_mgr.fence_count;
fence->type = type;
fence->shape = FENCE_SHAPE_CIRCLE;
fence->action = action;
fence->center = *center;
fence->radius = radius;
fence->min_altitude = min_alt;
fence->max_altitude = max_alt;
fence->buffer_distance = buffer;
fence->enabled = true;
geofence_mgr.fence_count++;
return fence->fence_id;
}
// 测试
void test_geofence(void)
{
// 设置起飞点
geofence_mgr.home_position.latitude = 39.9042;
geofence_mgr.home_position.longitude = 116.4074;
geofence_mgr.home_position.altitude = 0;
// 添加作业区域 (包含围栏)
GeoPoint_t work_area[] = {
{39.9050, 116.4070, 0},
{39.9050, 116.4080, 0},
{39.9030, 116.4080, 0},
{39.9030, 116.4070, 0},
};
add_polygon_fence(FENCE_TYPE_INCLUDE, FENCE_ACTION_LOITER,
work_area, 4, 5.0, 120.0, 10.0);
// 添加禁飞区 (排除围栏)
GeoPoint_t no_fly_center = {39.9040, 116.4075, 0};
add_circular_fence(FENCE_TYPE_EXCLUDE, FENCE_ACTION_BRAKE,
&no_fly_center, 20.0, 0, 500.0, 5.0);
// 测试位置
GeoPoint_t test_positions[] = {
{39.9040, 116.4075, 50.0}, // 在作业区内
{39.9060, 116.4075, 50.0}, // 超出作业区
{39.9040, 116.4075, 130.0}, // 超高
};
for (int i = 0; i < 3; i++) {
FenceCheckResult_t result = check_all_fences(&test_positions[i]);
printf("Position %d: lat=%.4f, lon=%.4f, alt=%.1f\n",
i, test_positions[i].latitude,
test_positions[i].longitude,
test_positions[i].altitude);
printf(" Violation: %s, Approaching: %s, Distance: %.1fm\n",
result.is_violation ? "YES" : "NO",
result.is_approaching ? "YES" : "NO",
result.distance_to_boundary);
if (result.required_action != FENCE_ACTION_NONE) {
execute_fence_action(result.required_action, &test_positions[i]);
}
printf("\n");
}
}基于深度学习的避障
┌─────────────────────────────────────────────────────────────────────────────┐
│ 深度学习避障系统架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【输入层】 │
│ ───────────────────────────────────────── │
│ │
│ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │
│ │ RGB相机 │ │ 深度相机 │ │ 激光雷达 │ │ 毫米波雷达 │ │
│ │ 640×480 │ │ 深度图 │ │ 点云 │ │ 距离/速度 │ │
│ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │
│ │ │ │ │ │
│ └────────────────┼────────────────┼────────────────┘ │
│ │ │ │
│ ▼ ▼ │
│ 【感知层】 ┌────────────────────────────┐ │
│ ───────────── │ 多传感器融合 │ │
│ │ │ │
│ │ - 时间同步 │ │
│ │ - 空间对齐 │ │
│ │ - 特征融合 │ │
│ └─────────────┬──────────────┘ │
│ │ │
│ ▼ │
│ 【检测层】 ┌────────────────────────────┐ │
│ ───────────── │ 深度学习网络 │ │
│ │ │ │
│ │ ┌────────────────────┐ │ │
│ │ │ 目标检测 (YOLO) │ │ │
│ │ │ - 障碍物类别 │ │ │
│ │ │ - 边界框 │ │ │
│ │ └────────────────────┘ │ │
│ │ │ │
│ │ ┌────────────────────┐ │ │
│ │ │ 深度估计 │ │ │
│ │ │ - 距离预测 │ │ │
│ │ │ - 3D重建 │ │ │
│ │ └────────────────────┘ │ │
│ │ │ │
│ │ ┌────────────────────┐ │ │
│ │ │ 语义分割 │ │ │
│ │ │ - 可通行区域 │ │ │
│ │ │ - 障碍物区域 │ │ │
│ │ └────────────────────┘ │ │
│ └─────────────┬──────────────┘ │
│ │ │
│ ▼ │
│ 【决策层】 ┌────────────────────────────┐ │
│ ───────────── │ 避障决策 │ │
│ │ │ │
│ │ ┌────────────────────┐ │ │
│ │ │ 风险评估 │ │ │
│ │ │ - 碰撞概率 │ │ │
│ │ │ - 威胁等级 │ │ │
│ │ └────────────────────┘ │ │
│ │ │ │
│ │ ┌────────────────────┐ │ │
│ │ │ 路径规划 │ │ │
│ │ │ - 避障路径 │ │ │
│ │ │ - 最优轨迹 │ │ │
│ │ └────────────────────┘ │ │
│ └─────────────┬──────────────┘ │
│ │ │
│ ▼ │
│ 【控制层】 ┌────────────────────────────┐ │
│ ───────────── │ 飞行控制 │ │
│ │ - 速度指令 │ │
│ │ - 姿态指令 │ │
│ └────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────────┘深度学习避障模型
python
"""
基于深度学习的无人机避障系统
包含:
1. 障碍物检测 (YOLO)
2. 深度估计 (单目/双目)
3. 端到端避障网络
"""
import torch
import torch.nn as nn
import torch.nn.functional as F
import numpy as np
from typing import List, Tuple, Optional
# ============================================
# 障碍物检测模块
# ============================================
class ObstacleDetector(nn.Module):
"""
基于YOLOv5的障碍物检测
检测类别:
- 其他无人机
- 鸟类
- 建筑物
- 树木
- 电线
- 人
- 车辆
"""
def __init__(self, num_classes=7, pretrained=True):
super().__init__()
# 使用预训练的YOLOv5
# self.model = torch.hub.load('ultralytics/yolov5', 'yolov5s', pretrained=pretrained)
# 简化版: 自定义轻量级检测网络
self.backbone = nn.Sequential(
# 下采样特征提取
nn.Conv2d(3, 32, 3, stride=2, padding=1),
nn.BatchNorm2d(32),
nn.ReLU(),
nn.Conv2d(32, 64, 3, stride=2, padding=1),
nn.BatchNorm2d(64),
nn.ReLU(),
nn.Conv2d(64, 128, 3, stride=2, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(),
nn.Conv2d(128, 256, 3, stride=2, padding=1),
nn.BatchNorm2d(256),
nn.ReLU(),
)
# 检测头
self.detect_head = nn.Sequential(
nn.Conv2d(256, 128, 1),
nn.ReLU(),
nn.Conv2d(128, num_classes + 5, 1), # 5 = x, y, w, h, conf
)
self.num_classes = num_classes
def forward(self, x):
"""
输入: [B, 3, H, W] RGB图像
输出: [B, num_anchors, 5+num_classes] 检测结果
"""
features = self.backbone(x)
detections = self.detect_head(features)
# 调整形状
B, C, H, W = detections.shape
detections = detections.permute(0, 2, 3, 1).reshape(B, -1, C)
return detections
def detect(self, image, conf_threshold=0.5, nms_threshold=0.4):
"""
执行检测并后处理
"""
self.eval()
with torch.no_grad():
detections = self(image)
# 应用sigmoid到置信度和类别概率
detections[..., 4:] = torch.sigmoid(detections[..., 4:])
# 过滤低置信度
conf = detections[..., 4]
mask = conf > conf_threshold
# NMS处理
# ...
return detections
# ============================================
# 深度估计模块
# ============================================
class DepthEstimator(nn.Module):
"""
单目深度估计网络
基于编码器-解码器结构
"""
def __init__(self):
super().__init__()
# 编码器 (下采样)
self.encoder = nn.Sequential(
self._conv_block(3, 32, 7, stride=2),
self._conv_block(32, 64, 5, stride=2),
self._conv_block(64, 128, 3, stride=2),
self._conv_block(128, 256, 3, stride=2),
self._conv_block(256, 512, 3, stride=2),
)
# 解码器 (上采样)
self.decoder = nn.Sequential(
self._upconv_block(512, 256),
self._upconv_block(256, 128),
self._upconv_block(128, 64),
self._upconv_block(64, 32),
self._upconv_block(32, 16),
)
# 深度输出
self.depth_out = nn.Sequential(
nn.Conv2d(16, 1, 3, padding=1),
nn.Sigmoid(), # 输出0-1，乘以最大深度
)
self.max_depth = 100.0 # 最大深度100米
def _conv_block(self, in_ch, out_ch, kernel, stride=1):
return nn.Sequential(
nn.Conv2d(in_ch, out_ch, kernel, stride=stride, padding=kernel//2),
nn.BatchNorm2d(out_ch),
nn.ReLU(inplace=True),
)
def _upconv_block(self, in_ch, out_ch):
return nn.Sequential(
nn.ConvTranspose2d(in_ch, out_ch, 3, stride=2, padding=1, output_padding=1),
nn.BatchNorm2d(out_ch),
nn.ReLU(inplace=True),
)
def forward(self, x):
"""
输入: [B, 3, H, W] RGB图像
输出: [B, 1, H, W] 深度图 (米)
"""
features = self.encoder(x)
decoded = self.decoder(features)
depth = self.depth_out(decoded) * self.max_depth
return depth
# ============================================
# 端到端避障网络
# ============================================
class AvoidanceNetwork(nn.Module):
"""
端到端避障决策网络
输入: RGB图像 + 深度图 + 当前状态
输出: 避障动作 (速度/方向)
"""
def __init__(self, state_dim=12):
super().__init__()
# 图像特征提取
self.image_encoder = nn.Sequential(
nn.Conv2d(4, 32, 7, stride=2, padding=3), # RGB + Depth
nn.ReLU(),
nn.MaxPool2d(2),
nn.Conv2d(32, 64, 5, stride=2, padding=2),
nn.ReLU(),
nn.MaxPool2d(2),
nn.Conv2d(64, 128, 3, stride=2, padding=1),
nn.ReLU(),
nn.AdaptiveAvgPool2d((4, 4)),
)
# 状态编码
self.state_encoder = nn.Sequential(
nn.Linear(state_dim, 64),
nn.ReLU(),
nn.Linear(64, 64),
nn.ReLU(),
)
# 决策网络
self.decision_net = nn.Sequential(
nn.Linear(128 * 16 + 64, 256),
nn.ReLU(),
nn.Dropout(0.3),
nn.Linear(256, 128),
nn.ReLU(),
nn.Dropout(0.3),
nn.Linear(128, 64),
nn.ReLU(),
)
# 输出头
# 速度输出: vx, vy, vz (归一化到[-1, 1])
self.velocity_head = nn.Sequential(
nn.Linear(64, 3),
nn.Tanh(),
)
# 危险度输出: 0-1
self.danger_head = nn.Sequential(
nn.Linear(64, 1),
nn.Sigmoid(),
)
def forward(self, rgb, depth, state):
"""
输入:
- rgb: [B, 3, H, W] RGB图像
- depth: [B, 1, H, W] 深度图
- state: [B, state_dim] 飞行状态 (位置、速度、姿态等)
输出:
- velocity: [B, 3] 避障速度指令
- danger: [B, 1] 危险度
"""
# 合并RGB和深度
rgbd = torch.cat([rgb, depth], dim=1)
# 图像特征
img_features = self.image_encoder(rgbd)
img_features = img_features.view(img_features.size(0), -1)
# 状态特征
state_features = self.state_encoder(state)
# 融合特征
combined = torch.cat([img_features, state_features], dim=1)
# 决策
decision_features = self.decision_net(combined)
# 输出
velocity = self.velocity_head(decision_features)
danger = self.danger_head(decision_features)
return velocity, danger
# ============================================
# 可通行区域分割
# ============================================
class TraversabilityNet(nn.Module):
"""
可通行区域语义分割
输出: 每个像素的可通行概率
"""
def __init__(self):
super().__init__()
# U-Net结构
self.enc1 = self._encoder_block(3, 64)
self.enc2 = self._encoder_block(64, 128)
self.enc3 = self._encoder_block(128, 256)
self.enc4 = self._encoder_block(256, 512)
self.center = nn.Sequential(
nn.Conv2d(512, 1024, 3, padding=1),
nn.ReLU(),
nn.Conv2d(1024, 1024, 3, padding=1),
nn.ReLU(),
)
self.dec4 = self._decoder_block(1024 + 512, 512)
self.dec3 = self._decoder_block(512 + 256, 256)
self.dec2 = self._decoder_block(256 + 128, 128)
self.dec1 = self._decoder_block(128 + 64, 64)
self.final = nn.Conv2d(64, 1, 1)
def _encoder_block(self, in_ch, out_ch):
return nn.Sequential(
nn.Conv2d(in_ch, out_ch, 3, padding=1),
nn.ReLU(),
nn.Conv2d(out_ch, out_ch, 3, padding=1),
nn.ReLU(),
)
def _decoder_block(self, in_ch, out_ch):
return nn.Sequential(
nn.Conv2d(in_ch, out_ch, 3, padding=1),
nn.ReLU(),
nn.Conv2d(out_ch, out_ch, 3, padding=1),
nn.ReLU(),
)
def forward(self, x):
# 编码
e1 = self.enc1(x)
e2 = self.enc2(F.max_pool2d(e1, 2))
e3 = self.enc3(F.max_pool2d(e2, 2))
e4 = self.enc4(F.max_pool2d(e3, 2))
# 中心
c = self.center(F.max_pool2d(e4, 2))
# 解码 (跳跃连接)
d4 = self.dec4(torch.cat([F.interpolate(c, e4.shape[2:]), e4], dim=1))
d3 = self.dec3(torch.cat([F.interpolate(d4, e3.shape[2:]), e3], dim=1))
d2 = self.dec2(torch.cat([F.interpolate(d3, e2.shape[2:]), e2], dim=1))
d1 = self.dec1(torch.cat([F.interpolate(d2, e1.shape[2:]), e1], dim=1))
# 输出
out = torch.sigmoid(self.final(d1))
return out
# ============================================
# 完整避障系统
# ============================================
class UAVAvoidanceSystem:
"""
完整的无人机避障系统
"""
def __init__(self, device='cuda'):
self.device = device
# 加载模型
self.detector = ObstacleDetector().to(device)
self.depth_estimator = DepthEstimator().to(device)
self.avoidance_net = AvoidanceNetwork().to(device)
self.traversability_net = TraversabilityNet().to(device)
# 设为评估模式
self.detector.eval()
self.depth_estimator.eval()
self.avoidance_net.eval()
self.traversability_net.eval()
# 参数
self.max_velocity = 5.0 # m/s
self.danger_threshold = 0.7
def process_frame(self, rgb_image: np.ndarray, state: np.ndarray) -> dict:
"""
处理单帧图像
Args:
rgb_image: [H, W, 3] RGB图像 (0-255)
state: [12] 飞行状态
Returns:
避障结果字典
"""
# 预处理
rgb = torch.from_numpy(rgb_image).float().permute(2, 0, 1).unsqueeze(0)
rgb = rgb / 255.0
rgb = rgb.to(self.device)
state_tensor = torch.from_numpy(state).float().unsqueeze(0).to(self.device)
with torch.no_grad():
# 1. 深度估计
depth = self.depth_estimator(rgb)
# 2. 障碍物检测
detections = self.detector(rgb)
# 3. 可通行区域
traversability = self.traversability_net(rgb)
# 4. 避障决策
velocity, danger = self.avoidance_net(rgb, depth, state_tensor)
# 后处理
velocity = velocity.cpu().numpy()[0] * self.max_velocity
danger = danger.cpu().numpy()[0, 0]
depth_map = depth.cpu().numpy()[0, 0]
traversability_map = traversability.cpu().numpy()[0, 0]
# 构建结果
result = {
'velocity_cmd': velocity, # [vx, vy, vz]
'danger_level': danger,
'depth_map': depth_map,
'traversability_map': traversability_map,
'detections': detections.cpu().numpy(),
'emergency_stop': danger > self.danger_threshold,
}
return result
def get_avoidance_action(self, result: dict) -> dict:
"""
根据避障结果生成飞行指令
"""
if result['emergency_stop']:
# 紧急停止
return {
'action': 'EMERGENCY_STOP',
'velocity': [0, 0, 0],
'priority': 'HIGH'
}
velocity = result['velocity_cmd']
danger = result['danger_level']
if danger > 0.5:
# 高危险，减速避障
return {
'action': 'AVOID',
'velocity': velocity.tolist(),
'priority': 'HIGH'
}
elif danger > 0.3:
# 中等危险，调整路径
return {
'action': 'ADJUST',
'velocity': (velocity * 0.5).tolist(),
'priority': 'MEDIUM'
}
else:
# 安全，正常飞行
return {
'action': 'NORMAL',
'velocity': [0, 0, 0], # 不干预
'priority': 'LOW'
}
# 训练数据集
class AvoidanceDataset(torch.utils.data.Dataset):
"""
避障训练数据集
"""
def __init__(self, data_path, transform=None):
self.data_path = data_path
self.transform = transform
# 加载数据列表
self.samples = [] # [(image_path, depth_path, state, label), ...]
def __len__(self):
return len(self.samples)
def __getitem__(self, idx):
image_path, depth_path, state, label = self.samples[idx]
# 加载数据
# ...
return {
'image': image,
'depth': depth,
'state': state,
'velocity_label': label['velocity'],
'danger_label': label['danger'],
}
# 训练函数
def train_avoidance_model():
"""
训练避障模型
"""
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
model = AvoidanceNetwork().to(device)
optimizer = torch.optim.Adam(model.parameters(), lr=1e-4)
# 损失函数
velocity_loss_fn = nn.MSELoss()
danger_loss_fn = nn.BCELoss()
# 数据加载器
# train_loader = ...
num_epochs = 100
for epoch in range(num_epochs):
model.train()
total_loss = 0
# for batch in train_loader:
# rgb = batch['image'].to(device)
# depth = batch['depth'].to(device)
# state = batch['state'].to(device)
# vel_label = batch['velocity_label'].to(device)
# danger_label = batch['danger_label'].to(device)
#
# optimizer.zero_grad()
#
# velocity, danger = model(rgb, depth, state)
#
# loss_vel = velocity_loss_fn(velocity, vel_label)
# loss_danger = danger_loss_fn(danger, danger_label)
# loss = loss_vel + 0.5 * loss_danger
#
# loss.backward()
# optimizer.step()
#
# total_loss += loss.item()
print(f"Epoch {epoch+1}/{num_epochs}, Loss: {total_loss:.4f}")
# 保存模型
torch.save(model.state_dict(), 'avoidance_model.pth')
if __name__ == '__main__':
# 测试
system = UAVAvoidanceSystem(device='cpu')
# 模拟输入
rgb_image = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
state = np.random.randn(12).astype(np.float32)
result = system.process_frame(rgb_image, state)
action = system.get_avoidance_action(result)
print(f"Danger level: {result['danger_level']:.2f}")
print(f"Velocity cmd: {result['velocity_cmd']}")
print(f"Action: {action}")通信协议与组网
┌─────────────────────────────────────────────────────────────────────────────┐
│ 多机通信架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【集中式通信】 │
│ ───────────────────────────────────────── │
│ │
│ 地面站 (中心节点) │
│ ┌───┐ │
│ │GCS│ │
│ └─┬─┘ │
│ ┌──────────┼──────────┐ │
│ │ │ │ │
│ ▼ ▼ ▼ │
│ UAV1 UAV2 UAV3 │
│ │
│ 优点: 简单，易管理 │
│ 缺点: 单点故障，带宽瓶颈 │
│ │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ 【分布式自组网 (Ad-hoc)】 │
│ ───────────────────────────────────────── │
│ │
│ UAV1 ◄──────► UAV2 │
│ ▲╲ ╱▲ │
│ │ ╲ ╱ │ │
│ │ ╲ ╱ │ │
│ │ ╲ ╱ │ │
│ ▼ ╲ ╱ ▼ │
│ UAV4 ◄──╳───► UAV3 │
│ ╱ ╲ │
│ ╱ ╲ │
│ ▼ ▼ │
│ UAV5 UAV6 │
│ │
│ 每个节点可以与多个邻居通信 │
│ 支持多跳转发 │
│ │
│ 优点: 无单点故障，可扩展 │
│ 缺点: 协议复杂，延迟不确定 │
│ │
└─────────────────────────────────────────────────────────────────────────────┘通信协议实现
c
/**
* 多机通信协议
*/
#include <stdint.h>
#include <string.h>
// 消息类型
typedef enum {
MSG_HEARTBEAT = 0x01, // 心跳包
MSG_STATE_BROADCAST = 0x02, // 状态广播
MSG_FORMATION_CMD = 0x03, // 编队指令
MSG_AVOIDANCE_ALERT = 0x04, // 避障告警
MSG_GEOFENCE_BREACH = 0x05, // 围栏违规
MSG_EMERGENCY = 0xFF, // 紧急消息
} MessageType_t;
// 消息头
typedef struct __attribute__((packed)) {
uint8_t magic[2]; // 0xFE, 0xED
uint8_t msg_type;
uint8_t src_id; // 源节点ID
uint8_t dst_id; // 目标节点ID (0xFF = 广播)
uint16_t seq; // 序列号
uint16_t payload_len; // 载荷长度
uint32_t timestamp; // 时间戳 (ms)
} MessageHeader_t;
// 状态广播消息
typedef struct __attribute__((packed)) {
MessageHeader_t header;
// 位置 (mm精度)
int32_t pos_x; // 北向位置 (mm)
int32_t pos_y; // 东向位置 (mm)
int32_t pos_z; // 高度 (mm)
// 速度 (mm/s)
int16_t vel_x;
int16_t vel_y;
int16_t vel_z;
// 姿态 (0.01度)
int16_t roll;
int16_t pitch;
int16_t yaw;
// 状态标志
uint8_t flight_mode;
uint8_t battery_percent;
uint8_t gps_fix;
uint8_t satellites;
uint16_t crc;
} StateBroadcastMsg_t;
// 避障告警消息
typedef struct __attribute__((packed)) {
MessageHeader_t header;
uint8_t threat_uav_id; // 威胁源ID
uint8_t threat_level; // 威胁等级
int32_t distance_mm; // 距离 (mm)
int16_t closing_speed; // 接近速度 (mm/s)
int16_t time_to_collision; // 碰撞剩余时间 (0.1s)
// 避让建议
int16_t avoid_vx; // 建议避让速度X (mm/s)
int16_t avoid_vy;
int16_t avoid_vz;
uint16_t crc;
} AvoidanceAlertMsg_t;
// CRC16计算
uint16_t calculate_crc16(const uint8_t *data, int len)
{
uint16_t crc = 0xFFFF;
for (int i = 0; i < len; i++) {
crc ^= data[i];
for (int j = 0; j < 8; j++) {
if (crc & 1) {
crc = (crc >> 1) ^ 0xA001;
} else {
crc >>= 1;
}
}
}
return crc;
}
// 广播状态
void broadcast_state(uint8_t self_id, const UAVState_t *state)
{
StateBroadcastMsg_t msg;
// 填充头部
msg.header.magic[0] = 0xFE;
msg.header.magic[1] = 0xED;
msg.header.msg_type = MSG_STATE_BROADCAST;
msg.header.src_id = self_id;
msg.header.dst_id = 0xFF; // 广播
msg.header.seq = get_next_seq();
msg.header.payload_len = sizeof(msg) - sizeof(MessageHeader_t) - 2;
msg.header.timestamp = get_timestamp_ms();
// 填充载荷
msg.pos_x = (int32_t)(state->x * 1000);
msg.pos_y = (int32_t)(state->y * 1000);
msg.pos_z = (int32_t)(state->z * 1000);
msg.vel_x = (int16_t)(state->vx * 1000);
msg.vel_y = (int16_t)(state->vy * 1000);
msg.vel_z = (int16_t)(state->vz * 1000);
msg.roll = (int16_t)(state->roll * 180.0 / M_PI * 100);
msg.pitch = (int16_t)(state->pitch * 180.0 / M_PI * 100);
msg.yaw = (int16_t)(state->yaw * 180.0 / M_PI * 100);
msg.flight_mode = get_flight_mode();
msg.battery_percent = get_battery_percent();
msg.gps_fix = get_gps_fix_type();
msg.satellites = get_satellite_count();
// 计算CRC
msg.crc = calculate_crc16((uint8_t*)&msg, sizeof(msg) - 2);
// 发送
radio_send((uint8_t*)&msg, sizeof(msg));
}
// 解析接收到的消息
int parse_message(const uint8_t *data, int len, void *msg_out)
{
if (len < sizeof(MessageHeader_t)) {
return -1;
}
MessageHeader_t *header = (MessageHeader_t*)data;
// 检查魔数
if (header->magic[0] != 0xFE || header->magic[1] != 0xED) {
return -2;
}
// 检查长度
int expected_len = sizeof(MessageHeader_t) + header->payload_len + 2;
if (len < expected_len) {
return -3;
}
// 检查CRC
uint16_t received_crc = *(uint16_t*)(data + expected_len - 2);
uint16_t calculated_crc = calculate_crc16(data, expected_len - 2);
if (received_crc != calculated_crc) {
return -4;
}
// 复制消息
memcpy(msg_out, data, expected_len);
return header->msg_type;
}总结
编队控制方法对比
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 领航-跟随 | 简单，易实现 | 单点故障 | 小规模编队 |
| 虚拟结构 | 队形稳定 | 灵活性差 | 表演、展示 |
| 行为法 | 适应性强 | 参数难调 | 复杂环境 |
| 一致性 | 分布式，鲁棒 | 收敛慢 | 大规模集群 |
防撞系统层级
| 层级 | 距离 | 数据源 | 响应时间 |
|---|---|---|---|
| 协同规划 | 任务前 | 轨迹 | - |
| 编队协调 | 50-200m | GPS广播 | 1s |
| 感知避障 | 5-50m | 雷达/视觉 | 100ms |
| 紧急避撞 | <5m | 多源融合 | 10ms |
深度学习避障流程
RGB图像 + 深度图
│
▼
┌─────────────────┐
│ 障碍物检测 │ → 类别、位置
│ 深度估计 │ → 距离
│ 可通行分割 │ → 安全区域
└────────┬────────┘
│
▼
┌─────────────────┐
│ 避障决策网络 │ → 速度指令、危险度
└────────┬────────┘
│
▼
飞行控制器核心技术要点：
- 编队控制：选择合适的控制方法，平衡复杂度和性能
- 防撞设计：多层防护，从规划到紧急避撞
- 电子围栏：GPS围栏+高度限制，多种动作响应
- 深度学习：端到端避障，实时性要求高
- 通信组网：自组网支持，消息可靠传输
希望这篇文章对理解无人机编队协同技术有帮助！有问题欢迎评论区交流~
参考资料：
- PX4 Autopilot 源码
- ArduPilot 源码
- 无人机集群协同控制理论
- ROS Navigation Stack