PX4飞控实战：TFmini激光雷达高度融合与跳变问题深度解析
1. TFmini激光雷达与PX4飞控的基础配置
第一次用TFmini激光雷达作为PX4飞控的高度源时，我踩了个大坑——飞机在离地0.5米悬停时，高度数据突然跳到了5米，差点炸机。后来发现，**EKF2_HGT_MODE=3（range sensor模式）**这个关键参数没设对。激光雷达要想正常工作，得先搞定硬件连接和参数配置这两座大山。
硬件连接其实很简单，TFmini通过串口（比如TELEM2）与飞控相连。但要注意，有些飞控默认没开启TFmini支持，得手动修改固件。我用的Holybro Pixhawk 4就需要在default.cmake文件里添加distance_sensor/tfmini这一行，重新编译固件。具体操作路径是：
/PX4/Firmware/boards/px4/fmu-v5/default.cmake参数配置才是重头戏，这几个参数必须检查：
- SENS_TFMINI_CFG：选择TFmini连接的串口，比如TELEM2
- EKF2_RNG_AID：设为1（Enabled），让飞控使用雷达数据辅助导航
- EKF2_RNG_A_VMAX：建议设为1.5m/s，超过这个水平速度就禁用雷达高度
- EKF2_RNG_A_HMAX：根据雷达量程设置，TFmini建议不超过10米
2. 高度跳变问题的现象与诊断
上周在户外测试时，飞机在1米高度悬停，突然QGC地面站显示高度值跳到了-3米，而雷达原始数据（distance_sensor话题）却稳定在1.02米。这种传感器数据与融合结果分离的现象，就是典型的高度跳变问题。
通过分析数据日志，我发现三个关键线索：
- 跳变时
ekf2_innovations中的range_finder数据突然归零 - 气压计数据（
vehicle_air_data）在跳变瞬间有剧烈波动 - 飞控的本地位置估计（
vehicle_local_position）Z轴出现阶梯状突变
用ulog_viewer.py工具绘制时序图能清晰看到，当雷达数据与气压计数据差异超过阈值时，EKF2滤波器会突然丢弃雷达数据，导致高度估计失控。这个问题在低空（<2米）尤其明显，因为此时气压计受地面效应和旋翼下洗气流影响最大。
3. EKF2滤波器的运行机制解析
PX4的高度融合算法就像个挑剔的美食家，它会根据EKF2_HGT_MODE参数决定主菜（主要高度源），再用其他传感器做配菜（辅助修正）。当选择range_sensor模式时，算法行为是这样的：
首先检查雷达数据有效性：
- 测量值是否在
EKF2_RNG_A_HMAX范围内 - 飞行速度是否低于
EKF2_RNG_A_VMAX - 数据更新率是否稳定（TFmini应该是100Hz）
- 测量值是否在
然后进行多源数据融合：
graph TD A[雷达原始数据] --> B{数据有效?} B -->|Yes| C[EKF2预测校正] B -->|No| D[切换至气压计] C --> E[输出融合高度]
当雷达数据突然无效时（比如强光干扰），滤波器会紧急切换至气压计，但低空气压数据本身不准，就会导致高度跳变。这就是为什么官方文档特别强调传感器冗余的重要性。
4. 五种实战解决方案对比
试遍了网上能找到的所有方法，最终这五种方案效果最明显：
物理架高方案（实测最稳）
- 把TFmini安装在距机臂末端50cm处
- 避免旋翼气流直接冲击雷达
- 安装角度略微前倾5-10度
参数调优方案：
param set EKF2_GND_MAX_HGT 0 param set EKF2_HGT_REF 1 param set EKF2_RNG_A_VMAX 1.2固件修改方案： 修改
ekf2_main.cpp中的_control_status.flags.rng_hgt判断逻辑，增加数据连续性的检查条件传感器冗余方案：
- 同时使用TFmini和超声波（MB1200）
- 在
distance_sensor话题中设置不同ID
数据滤波方案： 在
tfmini.cpp驱动层添加移动平均滤波：filtered_distance = 0.8 * filtered_distance + 0.2 * new_measurement;
方案对比表：
| 方案 | 稳定性 | 复杂度 | 适用场景 |
|---|---|---|---|
| 架高 | ★★★★★ | ★★☆ | 所有机型 |
| 调参 | ★★★☆ | ★☆☆ | 临时调试 |
| 改固件 | ★★★★ | ★★★★★ | 高级用户 |
| 冗余 | ★★★★☆ | ★★★☆ | 专业应用 |
| 滤波 | ★★★☆ | ★★☆ | 软件方案 |
5. 深度优化与异常处理
在解决跳变问题后，我还发现几个隐藏坑点：
地面效应补偿：当飞机离地<1米时，旋翼下洗流会使气压计读数偏高。可以通过修改local_position_estimator模块中的地形跟踪算法，增加雷达权重：
if hagl < 1.0: w_rng = 0.9 # 雷达权重提高到90%阳光干扰处理：TFmini在强光下可能失效。我的应对策略是：
- 给雷达加遮光罩
- 设置故障转移超时（
EKF2_RNG_DELAY=50ms） - 在
commander模块中添加传感器健康状态监控
温度漂移问题：冬夏温差会导致雷达零漂。建议：
- 每次上电后做地面校准（
sensor_rangefinder命令） - 定期检查
distance_sensor话题中的signal_quality值
最后分享一个诊断脚本，可以实时监控高度融合状态：
import px4tools log = px4tools.read_ulog('test.ulg') df = log['vehicle_local_position'] df['z_diff'] = df['z'] - df['distance_bottom'] df.plot(y=['z', 'distance_bottom', 'z_diff'])把TFmini架高50cm后，我的无人机现在能在1米高度稳定悬停，高度波动控制在±3cm以内。记得第一次成功时，QGC上那条平滑的高度曲线让我激动了半天——这就是工程师的快乐吧。