项目仓库:https://github.com/yicongl1/Drone-Delivery-2022

2022 年那台配送无人机,飞控是 PX4,上层是 Jetson Nano 上的 ROS。所有“自主”最终都落成一件事:以 20 Hz 的频率,把一个位置设定点发给飞控。这篇把当时踩过的五个坑按顺序写下来,每一个都来自“明明代码没错,但飞机不听话”。

坑一:先切模式,再解锁

OFFBOARD(外部控制)模式要求飞控持续收到来自机载计算机的设定点。如果解锁之后迟迟没有设定点进来,PX4 会认为外部链路不可靠,自动上锁——现象就是“解锁了,但螺旋桨没转起来”,或者刚离地就掉下来。

所以现场流程是固定的:

  1. 地面站先发 set_mode 切到 OFFBOARD;
  2. send_info_node 立刻开始以 20 Hz 发设定点;
  3. 再调用 mavros/cmd/arming 解锁。

官方文档里还有一条:解锁后 5 秒内必须进入 OFFBOARD。我们的流程天然满足,但这条规则决定了顺序不能反。

坑二:设定点一停,模式就掉

OFFBOARD 不是“设一次就一直有效”的状态,而是一个必须持续喂数据的模式。任何一次 1 秒的停顿——cin 等你输入航点、打印日志卡住、SSH 断流——都可能让飞控退出 OFFBOARD,飞机随即进入失控保护。

我们为此专门写了一个 send_info_node:只要飞机不是“已解锁且处于 OFFBOARD”这个状态,就以 20 Hz 持续发布速度设定点。它是一张安全网,代价很低(一个只发消息的节点),但没有它,有一次调试就因为在地面站里多敲了一行命令,飞机在半空掉出了 OFFBOARD。

坑三:设定点不能跳

这是整个项目里最影响观感的一条。向飞控发布一个距离 3 米的目标点,和让设定点每 50 ms 挪 5 厘米,飞控接收到的都是“位置”,但响应完全不同:前者是阶跃输入,姿态会猛冲、过冲、再修正;后者是一条平滑轨迹。

void add_target(int i) {
    for (int j = 0; j < 3; j++) {
        if (abs(next_point[j] - fly_point[i + 1][j]) > 0.3) {
            if (next_point[j] > fly_point[i + 1][j]) {
                next_point[j] -= change;
            } else {
                next_point[j] += change;
            }
        } else {
            next_point[j] = fly_point[i + 1][j];
        }
    }
}

change 取 0.02~0.05 m。剩下的距离小于 0.3 m 时直接吸附到目标点,否则永远差最后一点点。这个“阈值内吸附”的写法很土,但解决了“逐步逼近永远到不了”的问题。

坑四:到没到,判定方式不止一种

同一套代码里有三种“到达”判定,因为三种动作的物理含义不同:

  • 平飞到位:三轴误差都 ≤ 0.15 m;
  • 落地:垂直速度 < 0.1 m/s 且 高度 < 0.1 m(只看高度会被“贴地悬停”骗过去);
  • 可以投放:高度 < 0.2 m 且机身已经稳住。

降落这一段还有个取巧的地方:设定点的 z 一路压到 −0.5 m,也就是地表以下。

while (abs(vel_drone[2]) > 0.1 || pos_drone[2] > 0.1) {
    Offb_.send_local_pos_setpoint(land_pos);
    if (land_pos[2] > -0.5) land_pos[2] -= change;
    else land_pos[2] = -0.5;
}

飞控不可能真的降到 −0.5 m,于是一路保持向下的推力,直到速度和高度都掉进阈值。用“不可能到达的目标”换取“持续下压”,比写一个降落控制器简单得多。

坑五:投放是个时序问题,不是控制问题

投放看似最简单,其实最考验时序:

while (pos_drone[2] > 0.2) { /* 等落到 0.2 m 以下 */ }
// 保持位置 3 秒,让机身和货物彻底静下来
// 给 Arduino 发一个字节,舵机臂转开
// 再保持 2 秒,防止飞机提前拉起把货刮走

3 秒和 2 秒都是现场试出来的。短了,货物还在晃,落地会侧倾(规则要求竖直放置,倒了不算分);长了,5 分钟的赛时被吃掉一块。这类参数没有理论值,只能靠重复实验。

如果重来

  • 把 change 和 desire_z 收进一个参数文件。 当年它们散在七八个 launch 里,高度有 0.5 / 1.0 / 1.4 / 2.0 / 5.0 五个值,上机前要确认用的是哪个——这属于纯粹的隐患。
  • 给“是否在 OFFBOARD”加一个看门狗。 现在只有在终端里打印的模式,一旦掉模式,要等人肉发现。
  • 把设定点流和任务逻辑彻底分离。 com_node 一旦卡在 cin 上,就没有人发设定了;把发设定点交给独立线程或独立节点,任务逻辑怎么写都不会威胁飞行安全。

这三条的共同点:把“安全”从人的注意力里挪到结构里。无人机项目和纯软件项目最大的区别就在这里——纯软件的 bug 让你丢一个下午,无人机的 bug 让飞机掉下来。

代码在仓库里,包括那张 20 Hz 的设定点安全网:

https://github.com/yicongl1/Drone-Delivery-2022