快速鲁棒移动地平线估计方法（FR-MHE）
准确、高频的状态估计是四旋翼决策、规划与控制的基础，其性能直接关系到轨迹跟踪精度、飞行稳定性与任务安全。然而，轻量化无人机受尺寸、载荷与成本限制，机载计算和存储资源十分有限；与此同时，质量与升力系数偏差、未建模动态、阵风、载荷变化、外力冲击及桨叶损伤等内外扰动，又会显著降低状态估计精度。扩展卡尔曼滤波（EKF）计算效率较高，但在强非线性和复杂扰动下容易受到局部线性化误差影响；移动地平线估计（MHE）可在滑动时间窗口内统一利用系统模型、测量信息及约束，通常具有更好的非线性估计能力，却需要反复求解优化问题，难以满足微型无人机100 Hz闭环控制对实时性的要求。如何在资源受限硬件上兼顾估计精度、扰动鲁棒性与计算效率，是四旋翼自主飞行面临的重要问题。
为解决上述问题，东南大学自动化学院苏金亚/李世华教授团队(第一作者为其研一学生卢加衡)联合香港理工大学Wen-Hua Chen教授，开展了《FR-MHE：Fast and Robust Quadrotor State Estimation With Design, Analysis, and Validation》研究，提出快速鲁棒移动地平线估计方法（FR-MHE）。该方法将未知扰动显式增广为系统状态，并以单次梯度下降代替传统MHE的多次迭代优化，在保留扰动感知能力的同时，将估计计算压缩至亚毫秒级。研究成果发表于国际工业电子领域旗舰期刊《IEEE Transactions on Industrial Electronics》（中科院一区TOP期刊）。
一、研究方案：Protocol
本研究首先在四旋翼平移动力学中引入集总扰动力状态，将模型参数偏差、载荷变化、风扰、撞击及桨叶异常统一纳入增广模型，实现对位置、速度和扰动力的联合估计。随后，在每个移动时间窗口中利用上一时刻传播结果进行热启动，仅沿当前窗口代价函数的梯度方向完成一次局部校正；同时递推复用雅可比矩阵的中间计算结果，避免重复运算，使时间复杂度降至与鲁棒扩展卡尔曼滤波相近的量级。论文进一步给出了误差收缩条件及稳定性分析。估计结果输入Crazyflie原有串级PID控制器，无需改变底层控制结构。
*图1 Crazyflie四旋翼及实验系统总体示意图
*图2 FR-MHE状态估计与串级控制框架图
方案创新及优势
1. 通过扰动状态增广统一表征内外部不确定性，提升复杂扰动下的位置、速度与扰动力估计能力；
2. 采用热启动的单次梯度迭代和递推雅可比计算，实现亚毫秒级MHE，并给出相应稳定性条件；
3. 在资源受限的Crazyflie STM32F405微控制器上实现完全板载估计，并开展10类代表性对比实验。
二、实验验证：Validation
实验平台由Crazyflie 2.1微型四旋翼、四相机动作捕捉系统及上位机组成。其中，CHINGMU（青瞳视觉）MC1300光学动作捕捉系统以亚毫米精度获取无人机位置，并通过VRPN协议以100 Hz发送至上位机，再由Crazyradio转发至Crazyflie。状态估计在机载STM32F405微控制器上完成，位置、速度、扰动力与时间戳等数据经CRTP协议回传。上述数据链路均以100 Hz运行，为不同扰动场景下的闭环飞行和估计性能比较提供了可靠测量基础。
实验首先比较FR-MHE、传统鲁棒MHE（RMHE）与鲁棒EKF（REKF）的计算效率。当窗口长度N=8时，FR-MHE在PC端单次估计仅需0.17 ms，而RMHE需要19.23 ms；总体上，RMHE计算耗时为FR-MHE的55–126倍。FR-MHE部署至Crazyflie后单次计算为0.499 ms，估计频率超过500 Hz，缓存占用与CPU负载均保持较低水平。随后，研究设置了模型参数偏差、风扰、4.5 g附加载荷、多方向撞击、桨叶更换及桨叶裁剪等扰动，并完成悬停以及圆形、方形、三角形和双纽线轨迹飞行。结果表明，FR-MHE的估计精度整体接近RMHE，明显优于REKF；在风扰下，REKF悬停性能显著下降，而FR-MHE仍保持稳定；单个桨叶裁剪四分之一后，REKF控制下无人机失稳坠落，FR-MHE与RMHE均能完成飞行任务。
*图3 三种算法计算时间对比
*图4 三种算法计算时间结果
*图5 风扰下圆形、方形、三角形和双纽线轨迹飞行结果
三、实验成果：Result
该研究实现了面向资源受限微型四旋翼的快速鲁棒移动地平线估计，并通过青瞳视觉定位系统获得稳定、准确的闭环实验数据。十类对比实验验证了该方法在多种内外扰动下的估计精度、鲁棒性与实时性。与传统RMHE相比，FR-MHE实现55–126倍计算加速；与REKF相比，其计算量相近，但在复杂扰动下具有更好的估计性能与泛化能力，为MHE在轻量化机器人上的实际部署提供了可行路径。
参考文献：
[1] Jiaheng Lu, Jinya Su, Wen-Hua Chen, Shihua Li, “FR-MHE: Fast and Robust Quadrotor State Estimation With Design, Analysis, and Validation,” IEEE Transactions on Industrial Electronics, 2026, doi: 10.1109/TIE.2026.3708941.
[2] Wen-Hua Chen, Jun Yang, Lei Guo, and Shihua Li. "Disturbance-observer-based control and related methods—An overview." IEEE Transactions on industrial electronics 63, no. 2 (2015): 1083-1095.
[3] Jinya Su, Wen-Hua Chen, and Jun Yang. "On relationship between time-domain and frequency-domain disturbance observers and its applications." Journal of Dynamic Systems, Measurement, and Control 138, no. 9 (2016): 091013.
......
