关键词：绳挂负载、微分平坦、防摆控制、输入整形、视觉伺服、AprilTag、欠驱动系统
0. 结论先行
这个方向可行，但可行性对边界条件极其敏感。先给一张分级表：
| 场景 | 成熟度 | 说明 |
|---|---|---|
| 合作标志（AprilTag）+ 顺应式吊具 + 载荷 <2kg + 风速 <5m/s | 工程可落地，成功率可做到 90%+ | 现有开源栈拼装即可 |
| 非合作目标（任意物体）+ 已知类别 + 室内 | 需专门研发，6~12 个月 | 6D 位姿估计是瓶颈 |
| 任意物体 + 任意姿态 + 户外阵风 | 研究阶段 | MBZIRC 竞赛级难度 |
| 重载（>起飞重量 30%）长距离转运 | 电池是硬约束 | 续航断崖式下降 |
最关键的一句话：这个任务的难点不在视觉，而在吊挂负载的欠驱动动力学。
绝大多数团队第一版都栽在这里------视觉调得很准，一起飞就开始晃，越修正晃得越厉害。下面先把这个问题讲透，再展开方案。
1. 核心难点：绳挂负载是一个球面摆
1.1 共振问题
无人机 + 绳索 + 负载构成一个欠驱动系统，其摆动固有频率为：
ωn=gL \omega_n = \sqrt{\frac{g}{L}} ωn=Lg
| 绳长 L | 固有频率 ωn\omega_nωn | 摆动周期 T |
|---|---|---|
| 0.5 m | 4.43 rad/s | 1.42 s |
| 1.0 m | 3.13 rad/s | 2.01 s |
| 1.5 m | 2.56 rad/s | 2.45 s |
| 3.0 m | 1.81 rad/s | 3.47 s |
而四旋翼位置环的闭环带宽通常在 1~3 rad/s ，与上述摆动频率直接重叠。
后果很直接：位置控制器为了消除位置误差而产生的加速度指令，恰好落在摆动模态的激励频段上。你越想把飞机拉回目标点，摆动激励得越猛，形成正反馈，典型表现是发散振荡或极限环。
这不是调参能解决的问题，是结构性的，必须在架构层面处理。
1.2 三个衍生问题
（1）抓取瞬间的质量突变
负载离地那一刻，系统总质量阶跃增加。若不做推力前馈补偿，飞机会先掉高度再爬升，极易把负载拽翻、拖拽或撞地。
所需的额外推力可以直接前馈：
ΔT=mload⋅g \Delta T = m_{load} \cdot g ΔT=mload⋅g
在抓取确认信号触发的同一控制周期立即加入，靠位置环反馈去追是来不及的（响应延迟通常 200ms 以上，这段时间飞机已经掉了十几厘米）。
（2）质心与惯量改变
负载挂在机体下方，等效于把系统质心下移、转动惯量增大。姿态环增益需要在线调度（gain scheduling）或采用自适应控制，否则轻则响应变慢，重则失稳。
（3）下洗流干扰
螺旋桨下洗流会把轻质物体吹跑，同时扬起的灰尘/水花会直接污染下视相机。这也是后面强烈推荐用电动绞车的原因------让飞机在高处悬停，只放绳下去。
1.3 理论支点：微分平坦性依然成立
好消息是：Sreenath、Lee 与 Kumar 在 2013 年（CDC）证明了 带绳挂负载的四旋翼系统仍然是微分平坦的，平坦输出取为：
σ= xL, yL, zL, ψ T \sigma = \\,x_L,\\ y_L,\\ z_L,\\ \\psi\\,^T σ=xL, yL, zL, ψT
即 负载的位置 + 无人机偏航角。
这个结论的工程价值极大：
- 你可以直接对负载规划一条 minimum-snap 轨迹
- 无人机自身的期望位置、姿态、推力，都可以由负载轨迹及其各阶导数显式反解出来
- 只要负载轨迹足够光滑，摆动从一开始就不会被激励
也就是说，防摆的最优策略不是"事后压制"，而是"事前不激励"。这是整个方案的理论基础。
（注意：该结论成立的前提是绳索始终张紧。绳索松弛时系统会发生模态切换，这是一个混杂系统问题，工程上通过限制最大下降加速度 az<ga_z < gaz<g 来规避。）
2. 系统架构
分层结构如下，从上到下频率依次升高：
┌─────────────────────┐ ┌─────────────────────┐
│ 视觉目标定位 │ │ 摆角测量 │
│ 相机 + PnP 位姿解算 │ │ 吊点编码器 / IMU │
└──────────┬──────────┘ └──────────┬──────────┘
│ │
└───────────┬─────────────┘
▼
┌───────────────────────┐
│ 状态融合 │
│ 负载位置与摆角 EKF │
└───────────┬───────────┘
▼
┌───────────────────────┐
│ 负载轨迹规划 │
│ 微分平坦 · 10 Hz │
└───────────┬───────────┘
▼
┌───────────────────────┐
│ 防摆控制 │
│ 输入整形 + LQR · 50 Hz │
└───────────┬───────────┘
▼
┌───────────────────────┐
│ 姿态与推力内环 │
│ 飞控 · 500 Hz │
└───────────────────────┘关键点：被控对象是"负载"，不是"机体" 。整个控制链路的目标量应该是负载位置 pLp_LpL，机体位置只是中间变量。这是与常规四旋翼控制最本质的区别。
3. 五个层次的具体方案
3.1 机构层（性价比最高的一层）
不要指望视觉把精度做到 1cm。正确做法是用机构的顺应性去吃掉定位误差。
这是整个方案里投入产出比最高的一件事：
- 末端加弹簧 / 柔性关节 + 漏斗形导向罩，可以把允许误差从 1cm 放宽到 5cm。感知端的压力立刻下降一个数量级
- 抓取方式按实现难度排序：
| 方式 | 适用对象 | 优点 | 缺点 |
|---|---|---|---|
| 电磁铁 | 铁磁件 | 通电即抓，无活动部件，最省事 | 只能吸铁；断电即掉（安全隐患） |
| 气动吸盘 | 平整、无孔表面 | 通用性好 | 需气泵，增重；粗糙面失效 |
| 被动挂钩 + 主动脱扣 | 带吊环的物体 | 最轻，机构极简 | 要求目标预置吊环 |
| 欠驱动自适应机械爪 | 任意形状 | 通用性最好（Fin Ray / 绳驱三指） | 最重、最复杂、功耗最高 |
- 强烈建议配电动绞车 。无人机在 3~5m 高度稳定悬停不动，只放绳让吊具下降。这一个设计决策能把问题难度砍掉一半：
- 飞机远离地面，无地效干扰
- 下洗流不直接吹目标，不扬尘
- 撞击风险大幅降低
- 代价：绳长变化导致摆动频率时变，控制器需要按 L(t)L(t)L(t) 在线更新参数
3.2 感知层
起步一定用合作目标。 非合作目标的位姿估计会让你的项目周期至少翻倍。
- 推荐 AprilTag 36h11 + 多尺度嵌套标签：大标签管远距离识别，中心嵌套的小标签管近距离。否则接近到 30cm 时大标签必然超出视场，跟踪直接断掉
- 相机装在吊具上（eye-in-hand）优于装在机体上（eye-to-hand）。绳索摆动时机体与吊具不同步，装在机体上你还得先估出摆角才能换算吊具与目标的相对位姿，等于多引入一层误差
- 非合作目标路线 ：YOLO 检测 → RealSense / ToF 取深度 → 6D 位姿估计（点云 ICP 配准，或 FoundationPose 这类基于模型的网络）
- 注意：D435i 在户外强光下深度基本不可用。户外必须上双目 + 主动纹理投射，或直接用激光
- 最后 30cm 的盲区 ：目标必然被吊具本身遮挡，此时只能切换到 IMU 递推的开环段。这段开环时间越短越好，也是顺应性机构真正起作用的地方
视觉伺服方式选择：
| 方式 | 原理 | 适用 |
|---|---|---|
| PBVS（位置基） | 先估 6D 位姿，在笛卡尔空间闭环 | 直观，与轨迹规划天然衔接；但对相机标定误差敏感 |
| IBVS（图像基） | 直接在图像特征空间闭环 | 对标定误差鲁棒；但可能出现退化轨迹（相机后退） |
吊装场景推荐：远段 PBVS（便于做轨迹规划和防摆），近段可切 IBVS（对标定误差不敏感）。 如果机构顺应性做得好，全程 PBVS 也完全够用。
3.3 状态估计层
摆角能否测准，直接决定防摆效果的上限。三种方案的工程排序非常明确：
| 方案 | 成本 | 可靠性 | 建议 |
|---|---|---|---|
| 吊点装两轴编码器 / 万向节角度传感器 | 几十元 | 高（直接测量） | 首选，第一版就装上 |
| 下视相机视觉估计吊绳方向 | 零（复用相机） | 中（受光照、遮挡影响） | 作为冗余 |
| 机体加速度残差观测器（EKF / MHE 反推） | 零 | 低（对模型精度敏感，噪声大） | 别在这上面浪费时间 |
很多论文热衷于第 3 种"无需额外传感器"的方案，但在工程上，几十块钱的编码器带来的稳健性提升远超省下的成本。先把硬件装上，把问题变简单。
3.4 控制层
四种防摆手段，建议组合使用：
（1）输入整形（Input Shaping）------性价比之王
把原始指令与一串脉冲卷积，使各脉冲激起的振动相互抵消。ZV（Zero Vibration）整形器只需两个脉冲：
A1=11+K,A2=K1+K,K=e−ζπ/1−ζ2 A_1 = \frac{1}{1+K},\qquad A_2 = \frac{K}{1+K},\qquad K = e^{-\zeta\pi/\sqrt{1-\zeta^2}} A1=1+K1,A2=1+KK,K=e−ζπ/1−ζ2
两脉冲间隔取半个阻尼摆动周期：
Δt=Td2=πωn1−ζ2 \Delta t = \frac{T_d}{2} = \frac{\pi}{\omega_n\sqrt{1-\zeta^2}} Δt=2Td=ωn1−ζ2 π
对于绳挂系统，阻尼比 ζ\zetaζ 很小（0.01~0.05），此时 K≈1K \approx 1K≈1，两个脉冲幅值近似各占一半。
参考实现：
cpp
// ZV 输入整形器：对位置/速度指令做整形，抑制残余摆动
class ZVShaper {
public:
// L: 绳长(m)；zeta: 阻尼比；dt: 控制周期(s)
ZVShaper(double L, double zeta, double dt) { reset(L, zeta, dt); }
void reset(double L, double zeta, double dt) {
const double g = 9.81;
double wn = std::sqrt(g / L); // 固有频率
double K = std::exp(-zeta * M_PI / std::sqrt(1.0 - zeta * zeta));
A1_ = 1.0 / (1.0 + K);
A2_ = K / (1.0 + K);
double Td = 2.0 * M_PI / (wn * std::sqrt(1.0 - zeta * zeta));
delay_steps_ = std::max(1, int(std::round((Td / 2.0) / dt)));
buf_.assign(delay_steps_ + 1, 0.0);
head_ = 0;
}
// 每个控制周期调用一次
double filter(double cmd) {
buf_[head_] = cmd;
int tail = (head_ + 1) % buf_.size(); // 延迟 Td/2 的旧指令
double out = A1_ * cmd + A2_ * buf_[tail];
head_ = tail;
return out;
}
private:
double A1_ = 0.5, A2_ = 0.5;
int delay_steps_ = 1, head_ = 0;
std::vector<double> buf_;
};十几行代码，不需要任何反馈，不需要测摆角 ，只需要知道绳长。工业吊车几十年来的标配方案。第一版就应该上这个。
局限：对绳长估计误差敏感（误差 20% 时抑制效果显著下降），且只能抑制"残余"摆动，对外部扰动（阵风）引起的摆动无效。所以还需要下面几项。
（2）微分平坦轨迹规划
对负载规划 minimum-snap 轨迹，反解无人机轨迹。从源头上不激励摆动，是最优雅的方案。数学推导可参考 Sreenath & Kumar 的原文。
（3）摆角反馈 LQR
把摆角 θ\thetaθ 和摆角速度 θ˙\dot\thetaθ˙ 并入位置环状态向量，设计状态反馈。能处理外部扰动引起的摆动，是输入整形的必要补充。
（4）MPC
效果最好，能显式处理速度、倾角、绳张力等约束，并做有限时域预测。Jetson Orin 上可以跑到 20~50Hz。适合作为第二版方案。
推荐组合：轨迹层用微分平坦规划 + 指令层用输入整形 + 反馈层用摆角 LQR。 三者互补，实现成本可控。
3.5 任务状态机
搜索
→ 粗定位（3~5m 高度，广角视场）
→ 视觉伺服对准并下降
→ 悬停等摆角收敛（|θ| < 2° 且 |θ̇| < 5°/s，超时 10s）
→ 下放 / 接触
→ 抓取确认（电流突变 / 接触开关 / 夹持力）
→ 缓慢提升（同步施加推力前馈 ΔT = m_load · g）
→ 防摆转运
→ 目标点对准
→ 释放
→ 撤离每个状态都必须有超时和失败回退。 尤其注意两点：
- "抓取确认"绝不能只靠视觉判断。必须有物理反馈：电磁铁用电流突变，机械爪用夹持力或行程开关，吸盘用真空度。视觉误判"以为抓住了"然后飞机空手爬升，是最常见的失败模式
- 失败回退策略要预先定义清楚：目标丢失 → 悬停 5s 重搜索 → 上升扩大视场 → 放弃并返航。绝不能出现"卡在某个状态无限等待"的情况
4. 硬件与软件选型
| 环节 | 推荐方案 | 备注 |
|---|---|---|
| 飞控 | PX4 | offboard 模式成熟，社区有 slung load 相关工作 |
| 机载算力 | Jetson Orin Nano (8GB) | 若只跑 AprilTag，树莓派 5 也够 |
| 中间件 | ROS 2 Humble | --- |
| 视觉库 | OpenCV + apriltag 官方库 | 非合作目标加 TensorRT 部署的 YOLO |
| 深度相机 | RealSense D455（室内）/ 双目+主动纹理（户外） | D435i 户外强光下深度不可用 |
| 摆角传感 | 两轴磁编码器 或 吊点 IMU | 必装 |
| 仿真 | MuJoCo（首选）或 Gazebo + PX4 SITL | MuJoCo 的绳索与接触建模更准 |
5. 分阶段验证路线
每阶段设明确的通过判据，不达标不进入下一阶段：
| 阶段 | 内容 | 通过判据 |
|---|---|---|
| P0 | 仿真：MuJoCo 建绳挂模型，验证防摆算法 | 阶跃指令后 3s 内摆角收敛到 2° 以内 |
| P1 | 刚性挂载（短杆直连），室内 AprilTag 定位 | 定位重复精度 < 3cm |
| P2 | 加绳（固定长度 1m），只做悬停防摆 | 5m/s 侧向阵风下摆角 < 5° |
| P3 | 完整抓-提-放循环，静止目标 | 连续 20 次成功率 > 85% |
| P4 | 户外、绞车、移动或倾斜目标 | 按场景定 |
特别提醒：别跳过 P1。 很多团队图快，直接上绳做完整流程，结果视觉误差和摆动耦合在一起，出了问题根本分不清是感知的锅还是控制的锅，排查成本极高。先把变量隔离开。
6. 必须提前评估的硬约束
6.1 载荷与续航
吊装质量建议不超过整机起飞重量的 20~30%。超过之后续航下降是非线性的------不仅要克服额外重力，桨叶工作点偏离最优效率区，能耗会急剧上升。方案评估阶段就要按实际负载换算作业时间，而不是拿空载续航去算。
6.2 法规（可能直接卡死项目）
中国大陆的《无人驾驶航空器飞行管理暂行条例》对空中投放、吊挂作业有明确要求，涉及作业空域申请、适航与操控员证照、超视距飞行等。
建议在投入技术方案之前先过一遍法规，确认目标场景是否允许作业。这一项如果不通过，后面所有技术工作都是白做。
6.3 安全（最大的责任点）
负载脱落伤人是这个方向最严重的风险。几个设计原则：
- 锁止机构必须冗余 。电磁铁"断电即释放"是错误设计------一旦供电异常，负载直接掉落。应采用永磁保持 + 主动推脱，或加机械保险销
- 绳长限位：防止绳索过长缠绕螺旋桨
- 应急割绳装置：负载勾挂固定物时，飞机会被瞬间拽停甚至拉翻，必须能在毫秒级切断绳索
- 作业区域下方净空管控
6.4 致命失效模式清单
| 失效 | 后果 | 缓解措施 |
|---|---|---|
| 绳索缠绕螺旋桨 | 瞬间坠机 | 绳长限位 + 导向管 |
| 负载勾挂障碍物 | 拽停 / 拉翻 | 张力检测 + 应急割绳 |
| 摆动共振发散 | 失控 | 输入整形 + 摆角限幅保护 |
| 抓取误判空手爬升 | 任务失败（不致命） | 物理反馈确认 |
| 抓取瞬间掉高 | 撞地 / 拖拽 | 推力前馈补偿 |
| 绳索松弛（自由落体段） | 模态切换、控制失效 | 限制下降加速度 az<ga_z < gaz<g |
7. 小结
按重要性排序的四条建议：
- 先解决动力学，再解决视觉。摆动问题不处理，视觉再准也没用
- 用机构顺应性换感知精度。这是投入产出比最高的一步
- 摆角传感器必装。几十块钱换来的稳健性，比任何算法都实在
- 法规和安全前置评估。别做完再发现不能飞
技术上，"合作标志 + 顺应吊具 + 轻载 + 低风"这个组合现在就能做到工程可用水平；把任何一个条件放开，难度都会跳一个台阶。先把可控版本做扎实，再逐条放开约束，是最务实的推进路径。
参考文献与资源
- Sreenath, Lee, Kumar. Geometric Control and Differential Flatness of a Quadrotor UAV with a Cable-Suspended Load. CDC 2013.
- Mellinger, Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. ICRA 2011.
- Singer, Seering. Preshaping Command Inputs to Reduce System Vibration. 1990.（输入整形原始文献）
- MBZIRC 2017 / 2020 竞赛技术报告（空中抓取与搬运赛道）
- PX4 官方文档：offboard control 与自定义控制器接入