低空位置环：从期望轨迹到电机指令

多旋翼低空控制通常是级联结构：

轨迹生成 → 位置环 → 速度环 → 姿态环 → 角速率环 → 混控/电机

位置环在做什么

输入是期望位置 (p_d) 与当前估计 (p)，输出常为期望加速度或姿态倾角：

const e = pDesired.sub(p);
const eDot = vDesired.sub(v);
const aCmd = e.multiplyScalar(kp).add(eDot.multiplyScalar(kd)).add(aFeedforward);

低空场景额外约束：

• 高度通道更敏感（地面效应、视觉高度噪声）
• 水平通道受风扰明显
• 安全包络（最大倾角、最大爬升率）必须在进姿态环前裁剪

调参顺序

1. 角速率环（最快）
2. 姿态环
3. 速度环
4. 位置环
5. 轨迹时间尺度

先外环后内环，只会把内环缺陷放大成振荡。

延迟与采样

链路	典型
IMU	200–1000Hz
视觉/定位	10–50Hz
数传链路	5–20Hz（抖动大）

控制按本地高频跑，定位用观测器/滤波融合；不要把 UI 遥测频率和飞控频率绑死。

与可视化的接口

地面站只订阅“可解释状态”：

• pos, vel, att
• tracking_error
• mode / failsafe

Three.js 侧对 pos/att 做插值展示，对 tracking_error 做颜色映射，比直接画电机 PWM 更有用。