Mid-360S + FAST-LIO2 + MAVROS + PX4 真机部署测试

开始之前，已经完成了三件事：机载电脑连上并启动了 Mid-360S；FAST-LIO2 成功运行并输出 /Odometry（见《FAST-LIO2 从 0 跑通教程》）；机载电脑上的 MAVROS 能和 PX4 飞控通信。

这篇要做的是把 FAST-LIO2 的定位结果送进 PX4 的 EKF2，让 PX4 在没有 GPS 的室内也能得到稳定的本地位置估计，再用 Position 或 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 必须表达在机体系，而不是世界系。

二、安全检查

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

建议顺序：

第一次只上电，不装桨。
第二次装桨但固定机体或使用保护架。
第三次低高度、空旷室内、手动随时可切回 Stabilized 或 Altitude。
不要一开始就直接 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

类型应为 nav_msgs/msg/Odometry。重点看 header.frame_id、child_frame_id、pose.pose.position 和 pose.pose.orientation 这几个字段。

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

四、确认坐标方向

ROS 和 PX4 使用不同的坐标约定：ROS 机体系是 FLU（前、左、上），世界系是 ENU（x 为东或局部前方，y 为北或局部左方，z 向上）；PX4 机体系是 FRD（前、右、下），世界系是 NED。

ROS FLU 与 PX4 FRD 坐标约定对比：两者 X 都向前，ROS 的 Y 向左、Z 向上，PX4 的 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，可以跳过这一节。

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 包里找不到这个脚本，也可以手动下载：

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：

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

fcu_url 要换成实际的连接方式：

连接方式	`fcu_url` 示例
串口	`/dev/ttyUSB0:921600`
USB ACM	`/dev/ttyACM0:57600`
SITL UDP	`udp://:14540@127.0.0.1:14557`

检查连接：

1	ros2 topic echo /uav1/state

正常时 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，所以这样启动：

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

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

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

例如输入是 PoseStamped：

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 的 Vehicle Setup → Parameters 中修改下面几个参数。

1. 开启外部视觉融合

参数 EKF2_EV_CTRL：先开启 Horizontal position fusion 和 Vertical position fusion。FAST-LIO2 的 yaw 稳定、坐标方向也确认无误后，再考虑开启 Yaw fusion。

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

2. 设置高度参考

参数 EKF2_HGT_REF：室内主要依赖 Mid-360S / FAST-LIO2 定位时，设为 Vision。

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

3. 设置延迟

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

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

参数 EKF2_EV_POS_X、EKF2_EV_POS_Y、EKF2_EV_POS_Z：如果桥接节点发布的是无人机 base_link 或飞控中心的位姿，先都设为 0.0；如果直接发布的是雷达或 FAST-LIO IMU 坐标系的位姿，就要填雷达定位原点相对机体中心的位置。

注意这里按 PX4 机体系 FRD 填写：X 向前为正，Y 向右为正，Z 向下为正。例如雷达在飞控前方 0.08 m、左侧 0.02 m、上方 0.05 m 时：

参数	值
`EKF2_EV_POS_X`	`0.08`
`EKF2_EV_POS_Y`	`-0.02`
`EKF2_EV_POS_Z`	`-0.05`

5. 室内 GPS 处理

室内没有 GPS 或 GPS 信号很差时，不要让 GPS 参与位置融合，相关参数是 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

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

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：启动桥接节点

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 的 MAVLink Inspector 中查看 LOCAL_POSITION_NED 和 ODOMETRY 消息。设置 MAV_ODOM_LP = 1 可以让 PX4 回传收到的外部 odometry；方向和姿态检查完成后，再设回 MAV_ODOM_LP = 0。

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

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

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

九、先用 Position 模式悬停

第一次室内悬停推荐用 Position 模式而不是 Offboard：PX4 自己负责位置控制，遥控器可以直接接管，也更容易判断 EKF2 融合是否正常。

操作步骤：

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

如果 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，频率大于 2 Hz，实际建议 20 ~ 30 Hz。

可以先用 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 不融合

检查：

频率是否太低，建议 30 ~ 50 Hz。
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 提示。

重点确认 local position、local velocity 和 height 三项估计都是有效（valid）状态。

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

5. Offboard 一切换就退出

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

1	ros2 topic hz /uav1/setpoint_position/local

应大于 2 Hz，实际建议 20 Hz。还要检查 COM_OF_LOSS_T 和 COM_OBL_RC_ACT，它们决定 setpoint 丢失后的超时时间和保护动作。

十二、后续优化方向

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

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

参考资料

PX4 外部视觉定位文档：https://docs.px4.io/v1.16/en/ros/external_position_estimation
PX4 Offboard 模式文档：https://docs.px4.io/v1.16/en/flight_modes/offboard
MAVROS ROS 2 文档：https://mavros.readthedocs.io/en/latest/
MAVROS odometry 插件：https://mavros.readthedocs.io/en/latest/plugins/extras/odom/
MAVROS vision_pose 插件：https://mavros.readthedocs.io/en/latest/plugins/extras/vision_pose_estimate/
FAST-LIO 官方仓库：https://github.com/hku-mars/FAST_LIO
FAST-LIO2 ROS2 移植版本：https://github.com/Ericsii/FAST_LIO_ROS2