20 Hz 的设定点:PX4 OFFBOARD 的五个坑
项目仓库:https://github.com/yicongl1/Drone-Delivery-2022
2022 年那台配送无人机,飞控是 PX4,上层是 Jetson Nano 上的 ROS。所有“自主”最终都落成一件事:以 20 Hz 的频率,把一个位置设定点发给飞控。这篇把当时踩过的五个坑按顺序写下来,每一个都来自“明明代码没错,但飞机不听话”。
坑一:先切模式,再解锁
OFFBOARD(外部控制)模式要求飞控持续收到来自机载计算机的设定点。如果解锁之后迟迟没有设定点进来,PX4 会认为外部链路不可靠,自动上锁——现象就是“解锁了,但螺旋桨没转起来”,或者刚离地就掉下来。
所以现场流程是固定的:
- 地面站先发
set_mode切到 OFFBOARD; send_info_node立刻开始以 20 Hz 发设定点;- 再调用
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 的设定点安全网:
相关技能
- ROS
- C++
- Linux