大家好，今天我跟大家分享一个我最近做的一个项目：一个四旋翼无人机集群协同控制系统。

在这篇文章中，我将和大家一起来讨论6DOF建模、ADRC控制器设计、领导者-跟随者编队控制，以及基于MCP服务的AI辅助开发实践。

一、项目背景

无人机集群协同控制是当前无人机领域的研究热点，其核心挑战在于：

单机控制精度：四旋翼是欠驱动、强耦合的非线性系统

集群协调能力：多机之间需要保持相对位置并避免碰撞

抗扰动能力：实际飞行中存在风扰、负载变化等不确定因素

本项目选择 ADRC（自抗扰控制） 作为核心控制算法，原因在于其不依赖精确模型、抗扰能力强，非常适合无人机这类存在大量不确定性的系统。

二、系统架构

┌─────────────────────────────────────────────────────────┐
│                    集群控制系统                          │
├─────────────────────────────────────────────────────────┤
│  ┌─────────────┐    ┌─────────────┐    ┌─────────────┐  │
│  │  领导者无人机 │    │ 跟随者1号   │    │ 跟随者2号   │  │
│  │  (Leader)   │    │ (Follower1) │    │ (Follower2) │  │
│  └──────┬──────┘    └──────┬──────┘    └──────┬──────┘  │
│         │                  │                  │         │
│         └──────────────────┼──────────────────┘         │
│                            ▼                            │
│                  ┌─────────────────┐                    │
│                  │   编队控制器     │                    │
│                  └─────────────────┘                    │
└─────────────────────────────────────────────────────────┘

三、四旋翼无人机建模

3.1 坐标系定义

项目采用两种坐标系：

坐标系	说明
惯性坐标系	固定于地面，用于描述无人机绝对位置
机体坐标系	固连于无人机质心，用于描述姿态

3.2 六自由度运动方程

四旋翼的6DOF模型包含了：

位置方程：描述质心在惯性系中的运动

姿态方程：描述绕质心的旋转运动

我在这里给出这两个十分重要的方程，如下图所示：

位置动力学方程：

姿态动力学方程：

啰嗦一下：u1~u4为控制输入，分别对应总推力和三个方向的力矩。

关于这两个方程的具体推导，我在这里不做解释，大家有兴趣的话可以去查查相关的资料和文献，肯定比我讲得清楚。

3.3 关键参数（这个需要大家的大物有一定的基础）

% 无人机物理参数
params.mass = 1.5;          % 质量
params.Ixx = 0.0211;        % x轴转动惯量 (kg·m²)
params.Iyy = 0.0219;        % y轴转动惯量 (kg·m²)
params.Izz = 0.0413;        % z轴转动惯量 (kg·m²)
params.arm_length = 0.25;   % 机臂长度
params.gravity = 9.81;      % 重力加速度 (m/s²)

四、ADRC控制器设计

4.1 ADRC核心思想

传统PID控制存在以下局限：

• 依赖误差的微分信号，噪声敏感
• 线性组合限制了控制性能
• 无法主动补偿扰动

ADRC通过 "估计+补偿" 的思路解决上述问题：

┌────────────────────────────────────────────────────────┐
│                      ADRC结构                          │
├────────────────────────────────────────────────────────┤
│                                                        │
│   r(t) ──▶ [TD] ──▶ v1,v2 ──┐                         │
│                              │                         │
│                         [NLSEF] ──▶ u(t) ──▶ 被控对象  │
│                              │          │              │
│   y(t) ──▶ [ESO] ──▶ z1,z2,z3 ──┘      │              │
│             │                          │              │
│             └──────────────────────────┘              │
│                   (扰动估计补偿)                        │
└────────────────────────────────────────────────────────┘

4.2 三大核心组件（都涉及到了MATLAB语言，建议大家去了解一下）

（1）跟踪微分器（TD）

它的作用是：能够安排过渡过程，提取微分信号

function [v1, v2] = tracking_differentiator(v1, v2, r_target, r, h)
    % r: 快速因子，决定跟踪速度
    % h: 滤波因子
    fh = fhan(v1 - r_target, v2, r, h);
    v1 = v1 + h * v2;
    v2 = v2 + h * fh;
end

function fh = fhan(x1, x2, r, h)
    d = r * h^2;
    a0 = h * x2;
    y = x1 + a0;
    a1 = sqrt(d * (d + 8 * abs(y)));
    a2 = a0 + sign(y) * (a1 - d) / 2;
    sy = sign(y + d) - sign(y - d);
    a = (a0 + y - a2) * sy + a2;
    sa = sign(a + d) - sign(a - d);
    fh = -r * (a / d - sign(a)) * sa - r * sign(a);
end

（2）扩张状态观测器（ESO）

它的作用是：去估计系统状态和总扰动

function [z1, z2, z3] = eso_update(z1, z2, z3, y, u, beta, b0, dt)
    % z1: 输出估计
    % z2: 输出微分估计
    % z3: 总扰动估计
    e = z1 - y;
  
    z1 = z1 + dt * (z2 - beta(1) * e);
    z2 = z2 + dt * (z3 - beta(2) * fal(e, 0.5, delta) + b0 * u);
    z3 = z3 + dt * (-beta(3) * fal(e, 0.25, delta));
end

function f = fal(e, alpha, delta)
    if abs(e) <= delta
        f = e / (delta^(1-alpha));
    else
        f = abs(e)^alpha * sign(e);
    end
end

（3）非线性状态误差反馈（NLSEF）

它的作用是：去计算控制量

function u = nlsef(e1, e2, k1, k2)
    % e1: 位置误差
    % e2: 速度误差
    u = k1 * fal(e1, 0.5, delta) + k2 * fal(e2, 0.25, delta);
end

4.3 完整ADRC控制律

function u = adrc_control(y, y_ref, dt)
    persistent z1 z2 z3
  
    % 参数配置
    beta01 = 100;  % 观测器带宽
    beta02 = 300;
    beta03 = 1000;
    b0 = 0.5;      % 控制增益
    k1 = 50;       % 控制参数
    k2 = 100;
  
    % ESO误差
    e = z1 - y;
  
    % ESO更新
    z1 = z1 + dt * (z2 - beta01 * e);
    z2 = z2 + dt * (z3 - beta02 * fal(e,0.5,delta) + b0 * u);
    z3 = z3 + dt * (-beta03 * fal(e,0.25,delta));
  
    % 控制律（扰动补偿）
    u = (k1*(y_ref - z1) - k2*z2 - z3) / b0;
end

4.4 控制性能

性能指标	数值	说明
位置稳态误差	0.0001m	高精度定位
超调量	0.04%	几乎无超调
调节时间	0.645s	快速响应
抗扰恢复时间	<0.5s	强抗扰能力

五、集群编队控制

5.1 编队策略

采用领导者-跟随者架构：

在这里我们定义：

领导者：按预定轨迹飞行，接收上层指令

跟随者：根据与领导者的相对位置进行跟踪

5.2 相对运动学模型

跟随者 i 的期望位置由领导者位置和编队向量确定：   ，  其中为旋转矩阵，为编队偏移向量。

5.3 三种编队队形

我当时的想法是：我想要这群无人机至少有三种队列嘛（后续我会尝试给它加入更复杂的队列），然后就用了我们最常见的三角形，线性，钻石型（纯属于自己偏爱钻石），然后python代码也很简单，大家画图应该可以理解我这段python代码。当然，我这里还用到了列表，当然，很多专业大佬都用的是元组或自定义类（主要是列表简单啊）。。。

# 编队配置
FORMATION_TYPES = {
    'triangle': {
        'follower1': [0, -1.0, 0],    # 左后方
        'follower2': [0, 1.0, 0],     # 右后方
    },
    'line': {
        'follower1': [-1.0, 0, 0],    # 正后方
        'follower2': [-2.0, 0, 0],    # 更后方
    },
    'diamond': {
        'follower1': [-1.0, -1.0, 0], # 左后
        'follower2': [-1.0, 1.0, 0],  # 右后
    }
}

5.4 编队控制器

class FormationController:
    def __init__(self, formation_type='triangle'):
        self.formation_type = formation_type
        self.kp = 2.0  # 比例增益
        self.kd = 1.0  # 微分增益
      
    def compute_control(self, leader_state, follower_state, follower_id):
        # 计算期望位置
        desired_pos = self.get_desired_position(leader_state, follower_id)
      
        # 位置误差
        pos_error = desired_pos - follower_state['position']
      
        # 速度误差
        vel_error = leader_state['velocity'] - follower_state['velocity']
      
        # PD控制律
        control = self.kp * pos_error + self.kd * vel_error
      
        return control

5.5 编队保持性能

指标	数值
编队保持误差（RMS）	0.158m
最大偏差	0.42m
队形切换时间	2.3s

六、仿真实现

6.1 仿真环境

MATLAB R2023b：核心控制算法实现

Python 3.10：可视化与数据处理

NumPy/SciPy：数值计算

Matplotlib：结果绘图

6.2 仿真流程

# 主仿真循环
dt = 0.01  # 仿真步长
t_total = 30.0  # 总时长

for t in np.arange(0, t_total, dt):
    # 1. 更新领导者轨迹
    leader_trajectory = generate_reference(t)
  
    # 2. 计算跟随者期望位置
    for follower in followers:
        desired_pos = formation.get_desired(leader_trajectory, follower.id)
  
    # 3. ADRC控制计算
    for drone in all_drones:
        control = adrc.compute(drone.state, desired_state)
  
    # 4. 动力学更新
    for drone in all_drones:
        drone.state = dynamics.update(drone.state, control, dt)

6.3 仿真结果

3D编队飞行轨迹：

3D轨迹从图中可以观察到：

• 三架无人机保持稳定的相对位置
• 领导者（红色）沿预定轨迹飞行
• 跟随者（蓝色、绿色）准确跟踪

七、AI辅助开发实践

7.1 开发效率提升

本项目借助AI工具实现了显著的效率提升。（毕竟我现在的python水平只能说的上是初级水平）

7.2 MCP服务架构

项目搭建了12个MCP服务协同工作（主要是我个人比较喜欢这12个MCP）：

┌─────────────────────────────────────────────┐
│                 MCP服务集群                  │
├─────────────────────────────────────────────┤
│  ┌─────────┐  ┌─────────┐  ┌─────────┐     │
│  │ 文献检索 │  │ 代码生成 │  │ 公式推导 │     │
│  └─────────┘  └─────────┘  └─────────┘     │
│  ┌─────────┐  ┌─────────┐  ┌─────────┐     │
│  │ 数据分析 │  │ 图表绘制 │  │ 文档生成 │     │
│  └─────────┘  └─────────┘  └─────────┘     │
│  ┌─────────┐  ┌─────────┐  ┌─────────┐     │
│  │ 仿真调试 │  │ 参数优化 │  │ 结果验证 │     │
│  └─────────┘  └─────────┘  └─────────┘     │
│  ┌─────────┐  ┌─────────┐  ┌─────────┐     │
│  │ 版本管理 │  │ 质量检查 │  │ 翻译润色 │     │
│  └─────────┘  └─────────┘  └─────────┘     │
└─────────────────────────────────────────────┘

7.3 关键Prompt示例

 角色设定：
你是一位无人机控制领域的专家，精通ADRC理论和MATLAB实现。

任务：
帮我实现一个ADRC控制器，要求：
1. 包含TD、ESO、NLSEF三个模块
2. 参数可配置
3. 代码注释清晰

输出要求：
先给出整体架构图
再给出各模块代码
最后给出参数整定建议

八、项目成果

8.1 代码规模

语言	行数	说明
MATLAB	431行	核心控制算法
Python	471行	仿真与可视化
总计	902行	-

大家可以看到，像我们这种计算机科学与技术专业的学生，接触这种核心控制算法的机会是比较少的，特别是我们大一的（matlab像我们计算机科学与技术的用的其实很少），所以大家可以全当兴趣去了解一下，不用压力太大的。

8.2 学术产出

完整的IEEE格式论文

包含理论推导、仿真验证、结果分析

8.3 项目链接

在这里提供我的项目链接，有兴趣的朋友可以过来看一下：

资源	链接
GitHub仓库	https://github.com/ycx666994/quadrotor-cluster-control
技术博客	https://ycx666994.github.io
论文PDF	点击查看

九、总结与展望

9.1 主要贡献

1. 验证了ADRC在无人机控制中的有效性：实现了0.0001m的位置控制精度
2. 建立了完整的集群控制框架：支持多种编队队形和动态切换
3. 探索了AI辅助科研模式（因为我之前大部分是自主开发的）：开发效率提升约10倍

9.2 未来工作

1.引入视觉感知实现自主避障（推荐大家去学高翔老师的视觉slam14讲）

2.研究大规模集群的可扩展性

3.开展实物飞行验证

参考资料

1. 韩京清. 自抗扰控制技术[M]. 国防工业出版社, 2008.
2. Beard R W. Quadrotor Dynamics and Control[R]. BYU, 2008.
3. 相关系列博客：
   • ADRC控制原理详解
   • 从零搭建12个MCP服务
   • 四旋翼无人机建模与仿真

作者：ycx666994
创作时间：2026年
声明：本文为原创技术笔记，转载请注明出处

谢谢大家！！！