本文记录在自定义 Gazebo 场景（赤壁）中测试无人机视觉定位的过程：PX4 仿真一架搭载下视单目相机的 x500，相机图像桥接到 ROS 2 后，由图像匹配服务与预先生成的数据库比对，估计相机位姿。整套系统需要 6 个终端。

一、系统组成

Gazebo 赤壁场景

PX4 SITL：x500 + 下视单目相机

ros_gz_image：图像桥接到 ROS 2

MAVROS：无人机控制

图像匹配服务

匹配客户端：显示定位结果

Gazebo 赤壁场景

PX4 SITL：x500 + 下视单目相机

ros_gz_image：图像桥接到 ROS 2

MAVROS：无人机控制

图像匹配服务

匹配客户端：显示定位结果

终端 启动内容
1 Gazebo 赤壁场景（无界面）
2 PX4 无人机
3 相机图像桥接
4 MAVROS
5 图像匹配服务
6 匹配客户端

二、环境准备

ROS 2 Humble 与 Gazebo Harmonic 桥接时，需要安装 ros-humble-ros-gzharmonic，而不是 ROS 2 自带的 ros-humble-ros-gz：

1
sudo apt install ros-humble-ros-gzharmonic

三、启动仿真

终端 1：Gazebo 场景

1
2
3
4
5
# 设置 Gazebo 模型路径
export GZ_SIM_RESOURCE_PATH=$HOME/PX4-Autopilot/Tools/simulation/gz/models

# 启动仿真（-s 表示不启动图形界面，减少显存占用）
gz sim -s -r ~/PX4-Autopilot/Tools/simulation/gz/worlds/chibi.sdf

-s 为无界面模式，-r 表示启动后立即运行，chibi.sdf 是赤壁场景文件。

终端 2：PX4 无人机

1
2
3
4
5
6
7
cd ~/PX4-Autopilot

PX4_GZ_WORLD=chibi \
PX4_SYS_AUTOSTART=4001 \
PX4_SIM_MODEL=gz_x500_mono_cam_down \
PX4_GZ_MODEL_POSE="0,0" \
./build/px4_sitl_default/bin/px4 -i 0
参数 含义
PX4_GZ_WORLD=chibi 使用赤壁场景
PX4_SYS_AUTOSTART=4001 机型编号
PX4_SIM_MODEL=gz_x500_mono_cam_down 搭载下视单目相机的 x500
PX4_GZ_MODEL_POSE="0,0" 初始位置
-i 0 实例编号

四、接入 ROS 2

终端 3：桥接相机图像

1
2
3
4
ros2 run ros_gz_image image_bridge \
  /world/chibi/model/x500_mono_cam_down_0/link/camera_link/sensor/camera/image \
  --ros-args \
  -r /world/chibi/model/x500_mono_cam_down_0/link/camera_link/sensor/camera/image:=/my_camera/image_raw

Gazebo 中的相机图像会出现在 ROS 2 话题 /my_camera/image_raw 上。实际的 Gazebo 话题名取决于模型结构，可以先查一下：

1
gz topic -l | grep image

终端 4：MAVROS

1
2
3
4
5
6
# 设置 ROS 2 域 ID，避免与其他 ROS 2 系统冲突
export ROS_DOMAIN_ID=1

# 启动 MAVROS
ros2 launch onboard_control mainTree.launch.py \
  fcu_url:="udp://:14540@127.0.0.1:14557"

fcu_url 是飞控的 UDP 连接地址。

五、视觉定位

终端 5：图像匹配服务

1
2
3
4
5
6
7
cd ~/ros2_ws/src/onboard/headless_resection_service

# 设置 ROS 2 域 ID
export ROS_DOMAIN_ID=1

# 启动服务
bash start_ros_service.sh

服务端接收实时相机图像，与预先生成的数据库匹配，返回相机位姿估计。

终端 6：匹配客户端

1
2
3
4
5
# 设置 ROS 2 域 ID
export ROS_DOMAIN_ID=1

# 启动客户端
python3 test_client_ros_realtime.py

客户端订阅相机图像，向服务端发送匹配请求，并显示定位结果。

所有 ROS 2 节点都要使用同一个 ROS_DOMAIN_ID，否则彼此看不到对方的话题。

六、检查与可视化

检查 ROS 2 话题：

1
2
3
4
5
6
7
8
# 查看所有 ROS 2 话题
ros2 topic list

# 查看相机图像
ros2 topic echo /my_camera/image_raw --no-arr

# 查看匹配结果（根据实际话题名调整）
ros2 topic echo /matching/result

检查 Gazebo 话题：

1
2
3
4
5
# 列出所有 Gazebo 话题
gz topic -l

# 查看相机图像话题
gz topic -e -t /world/chibi/model/x500_mono_cam_down_0/link/camera_link/sensor/camera/image

用 RViz2 查看结果，添加 Image（相机图像）、Pose（定位位姿）和 TF（坐标系关系）三个显示项：

1
rviz2

七、常见问题

现象 检查与处理
收不到图像 用 gz topic -l | grep image 查实际话题名，据此修改桥接命令
节点之间无法通信 确认所有终端都设置了相同的 ROS_DOMAIN_ID
匹配服务没有响应 检查服务是否启动、数据库是否加载、网络通信是否正常
MAVROS 连接失败 检查 PX4 是否启动、UDP 端口是否正确，查看 MAVROS 日志

八、可以尝试的优化

  • 降低资源占用：使用无界面模式（上面已经使用 gz sim -s），降低相机分辨率，减少匹配时的特征点数量；
  • 提高定位精度：数据库要覆盖完整、光照均匀；按场景调整匹配相似度阈值；保证图像与 IMU 数据时间对齐。

参考资料