前言

前面已经完成了三件事：

  1. 机载电脑已经连上并启动 Mid-360S。
  2. FAST-LIO2 已经成功运行，并输出 /Odometry。
  3. 机载电脑上已经运行 MAVROS，可以和 PX4 飞控通信。

下一步要做的事情，就是把 FAST-LIO2 的定位结果送进 PX4 的 EKF2，让 PX4 在没有 GPS 的室内也能得到稳定的本地位置估计，然后使用 Position 模式或 Offboard 模式实现悬停。

本文先跑通最实用的一条链路：

1
2
3
4
5
6
7
8
9
Mid-360S
  -> livox_ros_driver2
  -> FAST-LIO2
  -> /Odometry
  -> external_odom_to_mavros
  -> /uav1/vision_pose/pose
  -> MAVROS
  -> PX4 EKF2
  -> Position / Hold / Offboard 悬停

一、整体思路

PX4 本身不直接订阅 ROS 2 的 /Odometry 话题。我们需要通过 MAVROS 把外部定位转换成 MAVLink 消息，再送给 PX4。

常见有两种接入方式：

接入方式 ROS 2 话题 MAVLink 消息 适合情况
位姿输入 /uav1/vision_pose/pose VISION_POSITION_ESTIMATE 最简单，先跑通悬停
里程计输入 /uav1/odometry/out ODOMETRY 可带速度和协方差，后期优化

本文先使用 /uav1/vision_pose/pose。它只需要 geometry_msgs/msg/PoseStamped，桥接逻辑简单，更适合第一次实机调通。

后续如果要进一步提升控制品质，可以改用 /uav1/odometry/out，但要特别注意：nav_msgs/Odometry 中的 twist 必须表达在机体系，而不是世界系。

二、安全检查

实机调定位和悬停前，先把安全检查放在第一位。

建议顺序：

  1. 第一次只上电，不装桨。
  2. 第二次装桨但固定机体或使用保护架。
  3. 第三次低高度、空旷室内、手动随时可切回 Stabilized 或 Altitude。
  4. 不要一开始就直接 Offboard 自动起飞。

室内定位悬停不是只看 /Odometry 有输出，还要确认坐标方向、尺度、延迟、PX4 融合状态都正确。

三、确认 FAST-LIO2 输出

启动 Mid-360S：

1
ros2 launch livox_ros_driver2 msg_MID360s_launch.py

启动 FAST-LIO2：

1
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml

检查 /Odometry：

1
2
3
ros2 topic info /Odometry
ros2 topic hz /Odometry
ros2 topic echo /Odometry --once

正常情况下类型应为：

1
nav_msgs/msg/Odometry

重点看消息头：

1
2
3
4
header.frame_id
child_frame_id
pose.pose.position
pose.pose.orientation

FAST-LIO2 的初始坐标原点一般是启动时的位置，室内悬停不要求它和真实世界坐标对齐，只要求局部坐标连续、方向正确、漂移足够小。

四、确认坐标方向

PX4 使用的是 FRD / NED 习惯：

1
2
3
4
机体系 FRD:
X 向前
Y 向右
Z 向下

ROS 常用的是 FLU / ENU 习惯：

1
2
3
4
5
6
7
8
9
机体系 FLU:
X 向前
Y 向左
Z 向上

世界系 ENU:
X 向东或局部前方
Y 向北或局部左方
Z 向上

MAVROS 会处理 ROS ENU/FLU 到 PX4 NED/FRD 的转换，所以我们发布给 /uav1/vision_pose/pose 的数据应当按 ROS 习惯来理解。

第一次调试时，可以把无人机拿在手里缓慢移动，观察 /Odometry：

1
ros2 topic echo /Odometry

推荐检查：

动作 ROS ENU 中推荐表现
抬高无人机 z 增大
向机头方向移动 x 增大
向机体左侧移动 y 增大

如果方向不一致，不要急着飞，需要先在桥接节点中做坐标转换，或者检查雷达安装方向、FAST-LIO2 外参和 TF。

五、安装 MAVROS 相关包

如果已经安装并启动 MAVROS，可以跳过这一节。

1
2
3
4
5
sudo apt update
sudo apt install -y \
  ros-humble-mavros \
  ros-humble-mavros-extras \
  ros-humble-mavros-msgs

安装 GeographicLib 数据：

1
ros2 run mavros install_geographiclib_datasets.sh

如果你的 MAVROS 包里找不到这个脚本，也可以手动下载：

1
2
3
4
cd /tmp
wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh
chmod +x install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh

启动 MAVROS：

1
2
3
4
5
source /opt/ros/humble/setup.bash
ros2 launch mavros px4.launch \
  fcu_url:="/dev/ttyUSB0:921600" \
  namespace:="uav1" \
  tgt_system:=1

如果你使用的是 USB、TELEM、UDP 或自定义 launch，需要把 fcu_url 换成自己的连接方式。

常见例子：

1
2
3
串口：/dev/ttyUSB0:921600
USB ACM：/dev/ttyACM0:57600
SITL UDP：udp://:14540@127.0.0.1:14557

检查连接：

1
ros2 topic echo /uav1/state

正常应看到：

1
connected: true

六、使用外部里程计到 MAVROS 的通用桥接包

1
git clone https://github.com/dreamer198/external_odom_to_mavros.git

编译：

1
2
3
source /opt/ros/humble/setup.bash
colcon build --packages-select external_odom_to_mavros --symlink-install
source install/setup.bash

FAST-LIO2 的输入话题是 /Odometry，消息类型是 nav_msgs/msg/Odometry，所以这样启动：

1
2
3
4
5
6
ros2 launch external_odom_to_mavros bridge.launch.py \
  input_type:=odometry \
  input_topic:=/Odometry \
  output_topic:=/uav1/vision_pose/pose \
  output_frame_id:=map \
  publish_rate:=30.0

检查输出：

1
2
ros2 topic hz /uav1/vision_pose/pose
ros2 topic echo /uav1/vision_pose/pose --once

推荐频率至少 30Hz。PX4 官方文档建议外部视觉消息以 30Hz 到 50Hz 输入，频率太低时 EKF2 可能不会融合。

如果后续换成 VINS、OpenVINS、LIO-SAM、UWB 或动捕，只需要改 input_type 和 input_topic。

例如输入是 PoseStamped：

1
2
3
4
ros2 launch external_odom_to_mavros bridge.launch.py \
  input_type:=pose_stamped \
  input_topic:=/external_pose \
  output_topic:=/uav1/vision_pose/pose

这个节点默认假设输入位姿已经符合 ROS ENU/FLU 习惯。如果方向检查不通过，需要先修正上游定位坐标系、传感器外参，或者在桥接前增加专门的坐标转换节点。

七、配置 PX4 EKF2 参数

打开 QGroundControl：

1
Vehicle Setup -> Parameters

重点修改下面几个参数。

1. 开启外部视觉融合

参数：

1
EKF2_EV_CTRL

建议先开启：

1
2
Horizontal position fusion
Vertical position fusion

如果 FAST-LIO2 的 yaw 方向稳定，并且坐标方向已经确认正确，再考虑开启：

1
Yaw fusion

如果只是先跑通室内悬停，不建议一开始就融合 velocity，因为本文使用的是 /uav1/vision_pose/pose，没有给 PX4 发送速度。

2. 设置高度参考

参数：

1
EKF2_HGT_REF

室内主要依赖 Mid-360S / FAST-LIO2 定位时，可以设为：

1
Vision

如果你的 FAST-LIO2 垂直方向还有漂移，可以先保留 Baro 做对比测试，但最终室内定点悬停通常希望视觉/激光定位成为主要高度来源。

3. 设置延迟

参数：

1
EKF2_EV_DELAY

第一次可以先设为：

1
0.0

如果飞行时出现明显的跟随滞后、刹车发飘、来回修正，再通过日志调这个值。这个延迟和雷达时间戳、FAST-LIO2 计算耗时、MAVROS 转发链路都有关系，每套机子都可能不同。

4. 设置外部定位传感器相对机体的位置

参数：

1
2
3
EKF2_EV_POS_X
EKF2_EV_POS_Y
EKF2_EV_POS_Z

如果桥接节点发布的是无人机 base_link 或飞控中心的位姿，可以先都设为：

1
0.0

如果你直接发布的是雷达或 FAST-LIO IMU 坐标系的位姿，就要填雷达定位原点相对机体中心的位置。

注意这里按 PX4 机体系 FRD 填：

1
2
3
X：向前为正
Y：向右为正
Z：向下为正

例如雷达在飞控前方 0.08m、左侧 0.02m、上方 0.05m：

1
2
3
EKF2_EV_POS_X = 0.08
EKF2_EV_POS_Y = -0.02
EKF2_EV_POS_Z = -0.05

5. 室内 GPS 处理

如果室内没有 GPS，或者 GPS 信号很差，建议不要让 GPS 参与位置融合。可以检查：

1
EKF2_GPS_CTRL

室内纯激光定位测试时，可将 GPS 融合关闭。不同 PX4 版本参数界面略有差别，建议通过 QGroundControl 的参数说明确认当前固件对应选项。

修改 EKF2 参数后，重启飞控。

八、检查 PX4 是否收到外部定位

启动顺序推荐如下：

终端 1：启动雷达

1
2
3
source /opt/ros/humble/setup.bash
source ~/livox_ws/install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360s_launch.py

终端 2：启动 FAST-LIO2

1
2
3
4
source /opt/ros/humble/setup.bash
source ~/livox_ws/install/setup.bash
source ~/fastlio_ws/install/setup.bash
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml rviz:=false

终端 3：启动 MAVROS

1
2
3
4
5
6
source /opt/ros/humble/setup.bash
ros2 launch mavros px4.launch \
  fcu_url:="/dev/ttyACM0:921600" \
  namespace:="uav1" \
  tgt_system:=1 \
  gcs_url:="udp://:14550@10.0.30.196:14550"

终端 4：启动桥接节点

1
2
3
4
5
6
7
8
source /opt/ros/humble/setup.bash
source ~/px4_lio_ws/install/setup.bash
ros2 launch external_odom_to_mavros bridge.launch.py \
  input_type:=odometry \
  input_topic:=/Odometry \
  output_topic:=/uav1/vision_pose/pose \
  output_frame_id:=map \
  publish_rate:=30.0

检查 MAVROS 本地位置：

1
ros2 topic echo /uav1/local_position/odom

如果 PX4 已经融合外部定位，这里应当能看到比较稳定的本地位置输出。

也可以在 QGroundControl 中查看：

1
2
MAVLink Inspector -> LOCAL_POSITION_NED
MAVLink Inspector -> ODOMETRY

为了让 PX4 回传收到的外部 odometry，可设置：

1
MAV_ODOM_LP = 1

方向和姿态检查完成后，建议再设回：

1
MAV_ODOM_LP = 0

然后手持无人机做方向检查：

动作 PX4 / MAVLink NED 中推荐表现
向机头方向移动 x 增大
向机体右侧移动 y 增大
抬高无人机 z 减小

如果这个方向不对，不要起飞。

九、先用 Position 模式悬停

最推荐的第一次室内悬停方式不是 Offboard，而是 Position 模式。

原因是：

  1. PX4 自己负责位置控制。
  2. 遥控器可以直接接管。
  3. 更容易判断 EKF2 融合是否正常。

操作步骤：

  1. 确认 /Odometry 稳定。
  2. 确认 /uav1/vision_pose/pose 约 30Hz。
  3. 确认 /uav1/state 中 connected: true。
  4. 确认 QGroundControl 没有位置估计相关的红色告警。
  5. 切到 Position 模式。
  6. 低高度起飞，建议先 0.3m 到 0.5m。
  7. 摇杆回中，观察是否能定点悬停。

如果 Position 模式无法进入，通常说明 PX4 认为本地位置估计还不可用。优先查：

1
2
3
ros2 topic hz /uav1/vision_pose/pose
ros2 topic echo /uav1/local_position/odom --once
ros2 topic echo /uav1/state

QGroundControl 中也要看 EKF2 是否有 innovation、height、local position 相关告警。

十、再测试 Offboard 悬停

Position 模式能稳定悬停以后，再测试 Offboard。

PX4 Offboard 有一个硬要求：进入 Offboard 前，外部控制端必须已经持续发送 setpoint，频率要大于 2Hz。实际使用建议 20Hz 或 30Hz。

可以先用 MAVROS 的位置 setpoint 话题做简单测试：

1
2
ros2 topic pub -r 20 /uav1/setpoint_position/local geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'map'}, pose: {position: {x: 0.0, y: 0.0, z: 0.8}, orientation: {w: 1.0}}}"

保持这个命令运行，再切 Offboard：

1
2
ros2 service call /uav1/set_mode mavros_msgs/srv/SetMode \
  "{base_mode: 0, custom_mode: 'OFFBOARD'}"

如果需要通过 MAVROS 解锁：

1
2
ros2 service call /uav1/cmd/arming mavros_msgs/srv/CommandBool \
  "{value: true}"

实机第一次不建议直接用命令解锁自动飞，最好先手动起飞到低高度，确认 Position 稳定后，再让 Offboard 接管当前位置。

十一、常见问题

1. /uav1/vision_pose/pose 有数据，但 PX4 不融合

检查：

  • 频率是否太低，建议 30Hz 到 50Hz。
  • EKF2 参数是否已开启 external vision fusion。
  • 修改参数后是否重启飞控。
  • 坐标方向是否正确。
  • 时间戳是否持续更新。
  • QGroundControl 是否有 EKF2 相关告警。

2. 起飞后上下抖动

优先检查：

  • EKF2_HGT_REF 是否合理。
  • FAST-LIO2 的 z 是否稳定。
  • 雷达是否看到了足够的地面、墙面或结构。
  • 机架震动是否影响 Mid-360S 或机载电脑。
  • EKF2_EV_DELAY 是否需要调大。

3. 水平位置慢慢漂移

常见原因：

  • 室内环境结构太少，比如空旷大厅、玻璃墙、长直走廊。
  • FAST-LIO2 外参不准。
  • 雷达安装不够刚性。
  • 飞机振动导致点云畸变或 IMU 质量变差。
  • 起飞前 FAST-LIO2 没有静止初始化。

4. 一切正常但 Position 模式无法进入

看 QGroundControl 的 Preflight / EKF 提示。

重点确认：

1
2
3
local position estimate valid
local velocity estimate valid
height estimate valid

如果没有 GPS，且飞控仍然要求 GPS，检查当前固件的 arming check 和 GPS 相关参数，不要盲目关闭所有安全检查。

5. Offboard 一切换就退出

PX4 Offboard 要求 setpoint 持续输入。检查：

1
ros2 topic hz /uav1/setpoint_position/local

应大于 2Hz，实际建议 20Hz。

还要检查：

1
2
COM_OF_LOSS_T
COM_OBL_RC_ACT

它们决定 Offboard 丢失后的超时和保护动作。

十二、后续优化方向

第一次能稳定悬停以后，可以继续优化：

  1. 把 /uav1/vision_pose/pose 改成 /uav1/odometry/out，向 PX4 发送完整 ODOMETRY。
  2. 加入速度估计，并确认速度表达在机体系。
  3. 精确标定 Mid-360S / FAST-LIO IMU 到机体中心的外参。
  4. 调整 EKF2_EV_DELAY，降低动态飞行时的创新误差。
  5. 录制 rosbag 和 PX4 ulog，对比 FAST-LIO2 输出与 PX4 local position。
  6. 在 Offboard 中加入当前位置锁定、限速、失联降落和遥控器接管逻辑。

参考资料