基于高通IQ-9075的跟随机器人实战：YOLOv8 + Re-ID + RGB-D + PID

HuiMinHuang2001

661人浏览 · 2026-09-21 11:42:05

HuiMinHuang2001 · 2026-09-21 11:42:05 发布

仿真图

在移动机器人项目里，“跟着我走”看起来是一个很简单的需求：

摄像头看到一个人，机器人跟着他移动。

但真正拆开后，它至少涉及五个问题：

谁是我要跟的人？
        ↓
这个人现在在哪里？
        ↓
距离机器人有多远？
        ↓
机器人应该向哪边转？
        ↓
机器人应该以多快的速度前进？

Qualcomm qrb_ros_samples 中的 sample_followme 正好把这几个问题串成了一条完整的 ROS 2 闭环：

RGB Camera
    ↓
YOLOv8 Person Detection
    ↓
Re-ID 身份确认
    ↓
Depth Camera 距离估计
    ↓
目标方向计算
    ↓
双 PID 控制
    ↓
/cmd_vel
    ↓
AMR 底盘

本文就从工程实现角度把它完整跑一遍。

01 项目背景：这次到底要做什么

1.1 实验平台

本次主平台选择：

Qualcomm Dragonwing IQ-9075 EVK

软件环境采用：

项目	本文配置
SoC	Qualcomm Dragonwing IQ-9075
AI 平台	Hexagon / HTP
系统	Qualcomm Ubuntu
ROS	ROS 2 Jazzy
目标检测	YOLOv8
AI Runtime	Qualcomm QNN
身份跟踪	Person Re-ID
RGB-D Camera	Gazebo / Orbbec Gemini 335L
控制	Dual PID
Robot Interface	ROS `/cmd_vel`

整个实战分成两部分：

A. Simulation
先证明算法链跑通

B. Real Robot
再接 RGB-D Camera + AMR 底盘

这样调试时不会把：

摄像头问题
AI 模型问题
Re-ID 问题
ROS 通信问题
底盘问题
PID 问题

全部混到一起。

1.2 本次最终目标

站在机器人正前方约 2 米的位置：

用户
 │
 │ 约 2m
 ▼
机器人

启动 Follow Me 后：

YOLO 检测到 person
判断画面中心区域只有一个候选人
获取这个人的 Re-ID 特征
将其设定为跟随目标
后续即使画面中出现其他人，也尽量继续匹配原目标
Depth Camera 持续计算人与机器人的距离
根据人物左右偏移计算方向角
PID 计算线速度与角速度
发布 /cmd_vel
机器人保持约 2m 距离持续跟随

最终形成闭环：

看见人
  ↓
认出人
  ↓
测距离
  ↓
算方向
  ↓
控制底盘
  ↓
机器人移动
  ↓
重新看见人

1.3 BOM 清单

仿真版本

设备	数量	用途
IQ-9075 EVK	1	YOLO、Re-ID、Follow Me
Ubuntu PC	1	Gazebo Simulation
千兆网线	1	ROS 2 DDS
RGB Camera	虚拟	Gazebo
Depth Camera	虚拟	Gazebo
AMR	虚拟	Gazebo

实机版本

设备	数量	用途
IQ-9075 EVK	1	主计算平台
Orbbec Gemini 335L	1	RGB + Depth
AMR 差速底盘	1	机器人运动
USB 3.0 Cable	1	RGB-D Camera
电源	1	EVK + Camera
急停按钮	1	实机安全
调试 PC	1	SSH / RViz / ROS 调试

1.4 配图建议

文章这一节不建议一上来放代码。

可以放一张 Qualcomm Robotics 平台演进相关截图：

RB5 / RB6 Robotics Platform
            ↓
Dragonwing IQ 系列
            ↓
Edge AI + Robotics

RB5/RB6 可以作为 Qualcomm 机器人生态的背景素材。

但本文真正运行 Sample 的硬件写 IQ-9075，不要把 RB5/RB6 写成本文实验设备。

02 Follow Me 到底是什么

Follow Me 与普通 YOLO 最大的区别是：

普通 YOLO：

画面里有没有人？

Follow Me：

我要跟的是哪一个人？
他现在在哪里？
距离是多少？
机器人应该怎么动？

因此：

YOLO ≠ Follow Me

YOLO 只负责：

Person Detection

Re-ID 负责：

Is this the same person?

Depth 负责：

How far?

PID 负责：

How should the robot move?

03 系统架构：RGB + Depth + YOLO + Re-ID + PID

整个系统可以画成下面这张图。

这才是 Follow Me 的完整闭环。

3.1 核心 ROS Topic

整个工程建议首先记住下面几个 Topic：

Topic	Type	用途
`/camera/color/image_raw`	Image	RGB 图像
`/camera/depth/image_raw`	Image	深度图
`/camera/color/camera_info`	CameraInfo	相机内参
`/yolo_detect_result`	Detection2DArray	YOLO 检测结果
`/target_visualization`	Image	跟随可视化
`/cmd_vel`	Twist/TwistStamped	机器人速度控制

另外还有两个关键 Re-ID Service：

/extract_feature

负责：

Person Crop
    ↓
Re-ID Network
    ↓
Feature Vector

以及：

/compute_similarity

负责比较：

Feature A
vs
Feature B

3.2 整条 ROS 数据流

这里还有一个非常重要的设计：

RGB
Depth
YOLO Result

并不是三个独立 Callback 各算各的。

Tracker 使用 message_filters 将三路消息做时间同步后再处理。

原因很简单。

如果：

RGB = t0

Depth = t0 + 500ms

YOLO = t0 - 300ms

这三份数据对应的根本不是同一个人物位置。

3.3 Follow Me 状态机

项目提供四个控制状态：

START
PAUSE
RESUME
FINISH

可以画成：

其中：

PAUSE

机器人立即：

linear = 0
angular = 0

但目标状态仍可继续恢复。

FINISH

除了停车，还会：

清除 Re-ID Template
+
清除当前 Target

下一次 START 就需要重新初始化一个人物。

3.4 目标初始化逻辑

这个设计值得仔细理解。

并不是画面里随便出现一个人就开始跟。

当前逻辑要求：

检测到 Person
       ↓
人物位于画面中央区域
       ↓
距离 1.5m ~ 3.0m
       ↓
符合条件的人恰好只有 1 个
       ↓
Extract Re-ID Feature
       ↓
保存 Initial Template
       ↓
目标初始化成功

流程：

这也是为什么第一次测试时最好：

只让一个人站在机器人正前方。

3.5 为什么还需要 Re-ID

考虑这个场景：

        Person B

Person A      Robot
  ↑
原始跟随目标

如果只有 YOLO：

person
person

两个 Bounding Box 在类别上完全一样。

机器人根本不知道：

哪个 person 才是原来的目标。

Re-ID 做的事情就是：

Person Crop
     ↓
Embedding
     ↓
与 Initial Template 比较
     ↓
是不是同一个人

3.6 双模板抗漂移机制

项目并不是只保存一个 Re-ID Feature。

可以理解成：

Initial Template
       +
Latest Template

Initial Template：

第一次锁定目标
长期保留

Latest Template：

目标当前较新的外观
动态更新

于是：

当前人物
   │
   ├── 和 Initial 比较
   │
   └── 和 Latest 比较

既能适应：

人物姿态变化
人物转身
人物远近变化

又可以通过 Initial Template 避免长期跟着跟着漂到另一个人身上。

3.7 Depth 是如何算距离的

首先获得 Person Bounding Box：

(x1, y1)
     ┌─────────┐
     │ Person  │
     │         │
     └─────────┘
              (x2, y2)

然后映射到 Depth Image。

代码并不是只读中心一个像素，而是读取 Bounding Box 内的有效深度。

例如：

2.03
1.98
2.01
8.40  ← 错误深度
1.99
2.02

最终采用：

Median

而不是 Average。

这样能明显降低离群值影响。

公式可以写成：

d_target = Median(D(x,y)), (x,y) ∈ PersonBBox

3.8 如何算人物位于左边还是右边

首先利用 CameraInfo 得到水平 FOV：

FOVx = 2 × atan(W / 2fx)

其中：

W  = 图像宽度
fx = 相机水平焦距

目标框中心：

u_target

图像中心：

u_center = W / 2

横向偏移：

offset = u_target - u_center

最后近似：

angle =
offset / u_center × FOVx / 2

于是：

angle < 0
→ 人在左边

angle ≈ 0
→ 人在中间

angle > 0
→ 人在右边

3.9 PID 控制机器人

Follow Me 中实际上存在两个控制环。

线速度 PID

控制：

离人太远还是太近

距离误差：

e_distance =
current_distance - target_distance

默认：

target_distance = 2.0m

例如：

当前距离 = 3m

error = 3 - 2
      = +1m

机器人应该向前。

角速度 PID

控制：

人物是否位于图像中心

e_angle = target_angle

然后：

linear_velocity
=
PID_distance(e_distance)

angular_velocity
=
PID_angle(-e_angle)

为什么有负号？

ROS 中：

angular.z > 0
→ 左转

angular.z < 0
→ 右转

而图像中目标位于右侧时：

angle_error > 0

所以需要：

angular.z < 0

04 项目部署：先仿真，再实机

这一部分开始正式动手。

建议按照下面这个顺序：

Step 1  Gazebo RGB-D
Step 2  ROS 2 DDS
Step 3  YOLO
Step 4  Re-ID
Step 5  Follow Me Tracker
Step 6  cmd_vel
Step 7  Real Robot

不要改变顺序。

4.1 固定代码版本

当前仓库 main 属于持续开发分支。

工程项目里第一件事情不是：

colcon build

而是：

git rev-parse HEAD

把 Commit 保存下来。

例如：

git rev-parse HEAD > BUILD_COMMIT.txt

以后出现：

昨天能跑
今天不能跑

至少知道软件到底发生了什么变化。

4.2 ROS 2 基础环境

两端都检查：

source /opt/ros/jazzy/setup.bash

echo $ROS_DISTRO

期望：

jazzy

本文统一：

export ROS_DOMAIN_ID=78

Host 与 IQ-9075 都必须一样。

4.3 Host 启动 Follow Me Simulation

创建 Workspace：

mkdir -p ~/qrb_ws/src
cd ~/qrb_ws/src

拉取：

git clone https://github.com/qualcomm-qrb-ros/qrb_ros_simulation.git

git clone https://github.com/qualcomm-qrb-ros/qrb_ros_samples.git

安装依赖：

cd ~/qrb_ws

source /opt/ros/jazzy/setup.bash

rosdep install \
    --from-paths src \
    --ignore-src \
    -r \
    -y

编译：

colcon build --symlink-install

加载：

source install/setup.bash

export ROS_DOMAIN_ID=78

进入 Simulation Follow Me 配置目录：

cd ~/qrb_ws/src/qrb_ros_samples/robotics/simulation_follow_me

启动：

ros2 launch qrb_ros_sim_gazebo \
    gazebo_robot_base_mini.launch.py \
    world_model:=warehouse_followme_path2 \
    rgb_camera_config_file:=$(pwd)/followme_rgb_camera_params.yaml \
    enable_laser:=false \
    enable_imu:=false

4.4 先别启动 AI，检查传感器

执行：

ros2 topic list | grep camera

重点确认：

/camera/color/image_raw
/camera/depth/image_raw
/camera/color/camera_info

RGB：

ros2 topic hz /camera/color/image_raw

Depth：

ros2 topic hz /camera/depth/image_raw

Camera Info：

ros2 topic echo \
    /camera/color/camera_info \
    --once

只要这一层失败：

不要继续调 YOLO。

4.5 检查 PC → IQ-9075 DDS

IQ-9075：

export ROS_DOMAIN_ID=78

ros2 topic list

如果能够看到：

/camera/color/image_raw
/camera/depth/image_raw

再执行：

ros2 topic hz \
    /camera/color/image_raw

以及：

ros2 topic hz \
    /camera/depth/image_raw

到这里说明：

Gazebo
   ↓
Ethernet
   ↓
DDS
   ↓
IQ-9075

链路已经成立。

4.6 准备 YOLOv8 QNN 模型

在支持 Qualcomm AI Hub Models 的环境导出 IQ-9075 QNN Context Binary。

思路：

YOLOv8
   ↓
QAI Hub
   ↓
QNN Context Binary
   ↓
IQ-9075 HTP

模型最终放置：

/opt/model/
├── yolov8_det_qcs9075.bin
└── coco8.yaml

例如：

sudo mkdir -p /opt/model

sudo cp yolov8_det_qcs9075.bin \
    /opt/model/

sudo cp coco8.yaml \
    /opt/model/

确认：

ls -lh /opt/model/

4.7 不要直接机械照抄 Orbbec YOLO Launch

这里是当前工程非常容易踩的第一个大坑。

Follow Me 必须要：

RGB
+
Depth
+
YOLO Detection

但是当前 Object Detection 示例中的 Orbbec Launch 配置会关闭 Depth。

因此对于 Follow Me，更推荐将：

Camera Source

与：

YOLO Pipeline

拆开。

也就是：

Gazebo / Orbbec
      │
      ▼
/camera/color/image_raw
      │
      ▼
YOLO Pipeline

而不是让 YOLO Launch 顺便负责启动 Camera。

4.8 创建 External Camera YOLO Launch

可以从：

launch_with_orbbec_camera.py

复制一份：

launch_external_camera.py

保留：

Preprocess
QNN Inference
YOLO Postprocess
Overlay

删除：

Include Orbbec Camera Launch

核心结构可以写成：

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():

    preprocess = ComposableNode(
        package="qrb_ros_cv_tensor_common_process",
        plugin=(
            "qrb_ros::cv_tensor_common_process::"
            "CvTensorCommonProcessNode"
        ),
        name="yolo_preprocess",
        parameters=[{
            "target_res": "640x640",
            "normalize": True,
            "tensor_fmt": "nhwc",
            "data_type": "float32",
        }],
        remappings=[
            (
                "input_image",
                "/camera/color/image_raw"
            ),
            (
                "encoded_image",
                "/qrb_inference_input_tensor"
            ),
        ],
    )

    inference = ComposableNode(
        package="qrb_ros_nn_inference",
        plugin=(
            "qrb_ros::nn_inference::"
            "QrbRosInferenceNode"
        ),
        name="yolo_inference",
        parameters=[{
            "backend_option":
                "/usr/lib/libQnnHtp.so",
            "model_path":
                "/opt/model/yolov8_det_qcs9075.bin",
        }],
        remappings=[
            (
                "qrb_inference_output_tensor",
                "/yolo_detect_tensor_output"
            )
        ],
    )

    postprocess = ComposableNode(
        package="qrb_ros_yolo_process",
        plugin=(
            "qrb_ros::yolo_process::"
            "YoloDetPostProcessNode"
        ),
        name="yolo_postprocess",
        parameters=[{
            "label_file":
                "/opt/model/coco8.yaml",
            "score_thres": 0.4,
            "iou_thres": 0.5,
        }],
    )

    container = ComposableNodeContainer(
        name="followme_yolo_container",
        namespace="",
        package="rclcpp_components",
        executable="component_container",
        composable_node_descriptions=[
            preprocess,
            inference,
            postprocess,
        ],
        output="screen",
    )

    return LaunchDescription([
        container
    ])

这样：

Simulation

和：

Real Orbbec

都可以复用同一个 YOLO Pipeline。

4.9 验证 YOLO

第一层：

ros2 topic hz \
    /camera/color/image_raw

第二层：

ros2 topic hz \
    /qrb_inference_input_tensor

第三层：

ros2 topic hz \
    /yolo_detect_tensor_output

第四层：

ros2 topic echo \
    /yolo_detect_result

最终应该能够看到：

class_id: person

或者对应：

class_id: 0

4.10 检查 Re-ID 服务

Follow Me 依赖外部 Re-ID Package。

先不要猜。

执行：

ros2 pkg prefix \
    qrb_ros_people_reid

存在后再启动：

ros2 launch \
    qrb_ros_people_reid \
    qrb_sample_people_reid.launch.py \
    reid_model_path:=/opt/model/osnet.bin

检查：

ros2 service list | grep feature

以及：

ros2 service list | grep similarity

期望能够看到：

/extract_feature
/compute_similarity

如果：

ros2 pkg prefix qrb_ros_people_reid

直接报 Package not found：

不要凭空找一个同名 GitHub 项目替代。

需要从对应 Qualcomm Robotics/QIRP 软件发布环境中获取与当前版本匹配的 Re-ID Package。

4.11 当前 main 分支源码需要注意 CMake

这里是第二个重要坑。

虽然：

src/person_tracker_node.cpp

存在完整代码，

但是当前 CMakeLists.txt 中 Tracker Target 的编译段处于注释状态。

所以如果你：

colcon build

然后发现：

ros2 run follow_me \
    person_tracker_node

找不到 executable，

不是你 ROS 学坏了。

4.12 源码构建修正

确保已经安装匹配的：

qrb_ros_people_reid

后，CMake 需要恢复类似：

find_package(qrb_ros_people_reid REQUIRED)

add_executable(person_tracker_node
    src/person_tracker_node.cpp
    src/template_pool.cpp
    src/pid_controller.cpp
    src/depth_processor.cpp
    src/state_machine.cpp
)

ament_target_dependencies(
    person_tracker_node

    rclcpp
    std_msgs
    sensor_msgs
    geometry_msgs
    vision_msgs
    cv_bridge
    image_transport
    message_filters
    qrb_ros_people_reid
)

target_link_libraries(
    person_tracker_node
    ${OpenCV_LIBRARIES}
)

rosidl_get_typesupport_target(
    cpp_typesupport_target
    ${PROJECT_NAME}
    rosidl_typesupport_cpp
)

target_link_libraries(
    person_tracker_node
    "${cpp_typesupport_target}"
)

install(
    TARGETS person_tracker_node
    DESTINATION lib/${PROJECT_NAME}
)

然后重新：

cd ~/qrb_ws

rm -rf \
    build/follow_me \
    install/follow_me

colcon build \
    --packages-select follow_me \
    --symlink-install

加载：

source install/setup.bash

检查：

ros2 pkg executables follow_me

应该能够看到：

follow_me person_tracker_node

4.13 启动 Tracker

Simulation：

ros2 launch follow_me \
    person_tracking.launch.py \
    use_sim_time:=true

真机：

ros2 launch follow_me \
    person_tracking.launch.py \
    use_sim_time:=false

4.14 启动 Follow Me

START：

ros2 service call \
    /follow_me/state_control \
    follow_me/srv/StateControl \
    "{set_state: 1}"

Pause：

ros2 service call \
    /follow_me/state_control \
    follow_me/srv/StateControl \
    "{set_state: 2}"

Resume：

ros2 service call \
    /follow_me/state_control \
    follow_me/srv/StateControl \
    "{set_state: 3}"

Finish：

ros2 service call \
    /follow_me/state_control \
    follow_me/srv/StateControl \
    "{set_state: 4}"

4.15 查看 Target Visualization

ros2 run rqt_image_view \
    rqt_image_view \
    /target_visualization

建议这里放文章截图：

![Follow Me Target Visualization](./assets/followme-target.png)

05 关键代码：Follow Me 到底是怎么跟人的

5.1 三路数据同步

Follow Me 同时依赖：

RGB
Depth
YOLO Detection

因此典型结构是：

using SyncPolicy =
    message_filters::sync_policies::ApproximateTime<
        sensor_msgs::msg::Image,
        sensor_msgs::msg::Image,
        vision_msgs::msg::Detection2DArray
    >;

然后：

RGB ──────┐
          │
Depth ────┼── Synchronizer
          │
YOLO ─────┘
              ↓
          Tracking

5.2 Person Filter

YOLO 输出可能包含：

person
chair
cup
laptop
car
...

Tracker 首先只保留：

person

逻辑可以抽象为：

for (const auto& detection : detections) {

    if (!isPerson(detection)) {
        continue;
    }

    person_boxes.push_back(
        toRgbCoordinate(detection.bbox)
    );
}

这里还存在一个分辨率转换：

YOLO Input
640 × 640

        ↓ Scale

Camera Image
640 × 360
或
640 × 480

所以 Bounding Box 需要重新映射回 Camera Image。

5.3 初始 Target 选择

我们自己把核心逻辑写得更容易读：

std::vector<Candidate> candidates;

for (const auto& bbox : person_boxes) {

    auto distance =
        getMedianDepth(bbox);

    if (!distance.has_value()) {
        continue;
    }

    if (*distance < 1.5 ||
        *distance > 3.0) {
        continue;
    }

    if (!insideCenterRegion(bbox)) {
        continue;
    }

    candidates.push_back({
        bbox,
        *distance
    });
}

if (candidates.size() != 1) {

    // 无法唯一确定目标
    return;
}

auto feature =
    extractReIdFeature(
        cropPersonImage(
            candidates[0].bbox
        )
    );

templatePool.initialize(feature);

这很好地解释了为什么：

画面中央同时站两个人

机器人反而不会初始化。

这是有意设计的。

5.4 Re-ID 匹配

进入 Tracking 后，每帧重新获得所有：

Person Detection

例如：

P1
P2
P3

可以先根据：

上一帧 Target BBox

计算 IoU。

优先判断空间位置最接近上一帧的人。

工程逻辑：

YOLO Person Candidates
        ↓
与上一帧 BBox 算 IoU
        ↓
按 IoU 从高到低排序
        ↓
依次 Extract Feature
        ↓
与 Re-ID Template 比较
        ↓
找到目标

这样避免：

每一帧
所有人
全部跑一次 Re-ID

降低计算量。

5.5 Re-ID Template 更新

可以抽象为：

if score <= update_threshold:
    latest_template = current_feature

匹配：

matched = (
    score_initial <= match_threshold
    or
    score_latest <= match_threshold
)

漂移：

if score_initial > drift_threshold:
    reject()

默认配置：

template_update_threshold: 0.1

match_threshold: 0.3

drift_threshold: 0.6

这里有一个非常值得提醒的地方：

当前代码把“数值越小”当成越接近。

所以虽然接口名字叫：

similarity

工程行为更接近：

distance score

如果以后自己把 Re-ID Service 改成真正的：

Cosine Similarity

通常：

数值越大越相似

那就不能直接继续使用：

score <= threshold

否则逻辑全部反了。

5.6 深度计算

推荐理解成：

std::vector<float> values;

for (pixel : person_bbox) {

    float d = readDepth(pixel);

    if (d > 0.1 &&
        d < 10.0) {

        values.push_back(d);
    }
}

float distance =
    median(values);

为什么不用 BBox 中心单点？

因为真实深度相机经常出现：

Median 通常会比单点稳定得多。

5.7 双 PID

标准 PID：

u(t)
=
Kp · e(t)
+
Ki · ∫e(t)dt
+
Kd · de(t)/dt

项目默认参数：

linear_kp: 0.5
linear_ki: 0.0
linear_kd: 0.1

angular_kp: 2.0
angular_ki: 0.0
angular_kd: 0.2

max_linear_speed: 0.5
max_angular_speed: 0.5

两个 PID：

5.8 建议增加控制死区

原始 PID 靠近目标时容易出现：

前一点
后一点
前一点
后一点

实际机器人建议增加 Dead Zone：

if (std::abs(distance_error) < 0.08) {
    distance_error = 0.0;
}

if (std::abs(angle_error) <
    3.0 * M_PI / 180.0) {

    angle_error = 0.0;
}

意思：

距离误差 < 8cm
→ 不再前后调整

角度误差 < 3°
→ 不再左右调整

对真实机器人体验帮助通常很明显。

5.9 PID 调参思路

现象	优先调整
机器人跟不上人	增大 Linear Kp
前后反复振荡	降低 Linear Kp / 增大 Kd
转向太慢	增大 Angular Kp
左右疯狂摆动	降低 Angular Kp
转向容易过冲	增大 Angular Kd
靠近 2m 仍抖动	加 Dead Zone
启动突然冲出去	降低 max_linear_speed

实机第一次建议不要直接：

max_linear_speed: 0.5

可以先：

max_linear_speed: 0.15

max_angular_speed: 0.30

确认方向全部正确以后再逐渐增加。

06 实际效果与工程验证

这里建议插入官方仓库中的：

simulation-followme.gif

文章资源目录：

![Simulation Follow Me](./assets/simulation-followme.gif)

6.1 不要只看“机器人动了”

验收建议拆成：

Level	内容	检查方法
L1	Camera	RGB/Depth 有数据
L2	YOLO	能检测 person
L3	Re-ID	两个 Service 正常
L4	Tracker	能初始化 Target
L5	Control	能输出速度
L6	Identity	多人时保持目标
L7	E2E	人移动后机器人跟随

6.2 L1 Camera

ros2 topic hz \
    /camera/color/image_raw

ros2 topic hz \
    /camera/depth/image_raw

6.3 L2 YOLO

ros2 topic echo \
    /yolo_detect_result

必须稳定出现：

person

6.4 L3 Re-ID

ros2 service list | grep -E \
    "extract_feature|compute_similarity"

至少：

/extract_feature
/compute_similarity

6.5 L4 Tracker

ros2 topic hz \
    /target_visualization

然后：

rqt_image_view

观察锁定框。

6.6 L5 cmd_vel

ros2 topic echo \
    /cmd_vel

测试人物站在右侧：

Robot
   \
    \
    Person

应该看到机器人产生：

右转方向 Angular Command

人离机器人 3 米：

linear > 0

约 2 米：

linear ≈ 0

6.7 推荐测试矩阵

Case	场景	期望
T01	单人在中央 2m	成功初始化
T02	单人在中央 4m	不初始化
T03	两人在中央	不初始化
T04	Target 向左走	Robot 左转
T05	Target 向右走	Robot 右转
T06	Target 远离	Robot 前进
T07	Target 靠近	Robot 后退/减速
T08	第二个人进入	仍追原 Target
T09	Target 短暂遮挡	等待重新匹配
T10	Target 完全丢失	Robot 停止
T11	Pause	立即停止
T12	Resume	恢复
T13	Finish	停车并清 Target

6.8 ROS Topic 图

运行：

rqt_graph

理想情况下可以看到：

camera
 ├─ color ──→ YOLO
 │              │
 │              ↓
 │        yolo_detect_result
 │              │
 └──────────────┼──→ follow_me
                │
depth ──────────┘

Re-ID Service
      ↑
      │
 follow_me
      │
      ▼
  /cmd_vel
      │
      ▼
 Robot Base

这里非常适合在文章中放一张真实的 rqt_graph 截图。

6.9 故障定位决策树

07 实机应用：从 Gazebo 换成真实 RGB-D Camera

仿真跑通以后，真正迁移实机反而比较简单。

原来：

Gazebo RGB
Gazebo Depth

替换成：

Orbbec RGB
Orbbec Depth

后面的：

YOLO
Re-ID
Tracker
PID

保持一致。

7.1 Orbbec Gemini 335L

启动 Camera：

ros2 launch \
    orbbec_camera \
    gemini_330_series.launch.py \
    enable_depth:=true \
    depth_registration:=true

这里建议：

depth_registration = true

非常重要。

7.2 为什么 Depth Registration 非常关键

当前 Follow Me 的深度实现，本质上是：

RGB BBox
   ↓
按照分辨率比例
   ↓
Depth BBox

即：

x_depth =
x_rgb × depth_width / rgb_width

它没有在 Tracker 中自行完成完整的：

RGB Camera Intrinsic
+
Depth Camera Intrinsic
+
Extrinsic
+
3D Reprojection

因此如果 RGB 和 Depth 没有对齐：

RGB:

     ┌ Person ┐
     │        │
     └────────┘


Depth:

             ┌ Person ┐
             │        │
             └────────┘

你取到的可能根本不是人物深度。

所以真机：

优先使用 Registered/Aligned Depth。

7.3 实机不要一次全部启动

推荐调试顺序：

① Camera

确认：

RGB
Depth
CameraInfo

然后：

② YOLO

确认：

person

然后：

③ Re-ID

确认：

ExtractFeature
ComputeSimilarity

然后：

④ Tracker

此时：

先不要让轮子着地。

只观察：

/cmd_vel

最后：

⑤ Base

才真正让机器人运动。

7.4 一个当前必须注意的 cmd_vel 接口差异

当前 Follow Me 源码发布：

geometry_msgs/TwistStamped

而不少 ROS AMR 底盘，包括当前 QRB Robot Base 接口，使用：

geometry_msgs/Twist

先检查：

ros2 topic type /cmd_vel

不要等到：

Follow Me 明明有速度
机器人就是不走

才发现消息类型不匹配。

7.5 推荐使用一个 Adapter，而不是直接乱改上游代码

例如让 Follow Me 输出：

/follow_me/cmd_vel_stamped

再增加转换器：

#!/usr/bin/env python3

import rclpy

from rclpy.node import Node

from geometry_msgs.msg import (
    Twist,
    TwistStamped,
)


class CmdVelAdapter(Node):

    def __init__(self):

        super().__init__(
            "follow_me_cmd_vel_adapter"
        )

        self.pub = self.create_publisher(
            Twist,
            "/cmd_vel",
            10,
        )

        self.sub = self.create_subscription(
            TwistStamped,
            "/follow_me/cmd_vel_stamped",
            self.callback,
            10,
        )

    def callback(self, msg):

        out = Twist()

        out.linear.x = msg.twist.linear.x
        out.linear.y = msg.twist.linear.y
        out.linear.z = msg.twist.linear.z

        out.angular.x = msg.twist.angular.x
        out.angular.y = msg.twist.angular.y
        out.angular.z = msg.twist.angular.z

        self.pub.publish(out)


def main():

    rclpy.init()

    node = CmdVelAdapter()

    rclpy.spin(node)

    node.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()

这样系统边界很清楚：

Follow Me
TwistStamped
      ↓
Adapter
      ↓
Twist
      ↓
Robot Base

7.6 实机安全

Follow Me Demo 直接产生速度控制。

它本身不是完整的安全导航系统。

也就是说：

Person
   ↑
   │
Robot ───────→ cmd_vel

并不等价于：

Obstacle Avoidance
+
Collision Monitor
+
Local Planner
+
Safety Controller

因此真实测试必须至少做到：

低速
+
空旷场地
+
急停
+
旁边有人看护

第一次运行：

max_linear_speed: 0.15

max_angular_speed: 0.30

通常比直接 0.5 更合适。

08 当前源码最容易踩的 10 个坑

坑 1：直接 colcon build 后没有 person_tracker_node

原因：

当前 main 的 CMake Target 被注释

解决：

恢复 CMake
+
准备 Re-ID Dependency
+
重新编译

坑 2：找不到 qrb_ros_people_reid

Follow Me 本身不是一个完全孤立的仓库。

它依赖外部 Re-ID Service。

先：

ros2 pkg prefix qrb_ros_people_reid

找不到就先解决依赖。

坑 3：照着 Object Detection Launch 启动后没有 Depth

检查：

ros2 topic list | grep depth

Follow Me 没有：

/camera/depth/image_raw

一定跑不完整。

坑 4：真机人物距离完全不对

优先检查：

RGB / Depth Registration

而不是先修改 PID。

因为：

错误 Depth
→ 错误 Distance
→ 错误 PID

PID 再好也救不了错误传感器数据。

坑 5：RGB、Depth、YOLO 都有，但是 Tracker 没反应

检查 Header Timestamp：

ros2 topic echo \
    /camera/color/image_raw \
    --field header

ros2 topic echo \
    /camera/depth/image_raw \
    --field header

ros2 topic echo \
    /yolo_detect_result \
    --field header

当前同步窗口非常严格。

源码实际配置接近：

10ms

如果 YOLO Pipeline 带来明显延迟，三路消息可能永远匹配不到。

调试阶段可以尝试：

sync_->setMaxIntervalDuration(
    rclcpp::Duration::from_seconds(
        0.05
    )
);

即：

50ms

但窗口也不是越大越好。

窗口太大会导致：

旧 YOLO BBox
+
新 Depth

被错误配到一起。

坑 6：日志和实际 Synchronizer 参数不一致

看代码时不要只相信：

RCLCPP_INFO(...)

当前代码中同步器实际配置和某些日志文字存在差异。

工程调试应该看：

真正构造函数参数

而不是只看打印信息。

坑 7：Re-ID “similarity” 方向理解反了

项目逻辑：

Score 越小
→ 越相似

如果换了自己的 Re-ID：

Cosine Similarity 越大越相似

需要同步修改：

match
update
drift

所有比较逻辑。

坑 8：画面两个人时无法初始化

这是设计行为。

第一次目标初始化要求：

中央有效 Candidate = 1

所以：

两个同事一起站机器人前面

然后说：

怎么不跟？

其实代码没坏。

坑 9：Follow Me 有 cmd_vel，但底盘不动

执行：

ros2 topic info /cmd_vel -v

重点检查：

Message Type
Publisher
Subscriber

尤其：

Twist
vs
TwistStamped

坑 10：把 Follow Me 当 Nav2

Follow Me 目前核心思路：

视觉
↓
PID
↓
cmd_vel

它并不是：

视觉
↓
目标坐标
↓
Nav2
↓
Obstacle Avoidance
↓
cmd_vel

两种架构安全边界完全不一样。

09 实机应用案例应该如何写

这里可以放你规划的：

Qualcomm Symposium Follow-Me Robot 视频

但是正文措辞建议写成：

“同类 Follow-Me 机器人应用展示”

不要写：

“这就是本文 sample_followme 的官方实机效果。”

如果公开 Symposium Demo 使用的是：

Pose Estimation
+
Stereo Depth

而本文实现是：

YOLO
+
Re-ID
+
Depth
+
PID

二者的产品目标类似，但技术实现并不完全相同。

工程文章必须把：

案例展示

和：

源码实现

分开。

10 扩展思路：从 Follow Me Demo 走向真正机器人能力

你给出的四个方向其实都非常合适：

Gesture
VLM
Voice
Navigation

我建议最后画成：

10.1 Gesture：抬手告诉机器人“跟我”

当前初始化条件是：

中央只有一个人
+
距离合适

可以升级成：

检测到多人
        ↓
Pose Estimation
        ↓
哪个人在举手
        ↓
锁定该人的 BBox
        ↓
Extract Re-ID
        ↓
Follow

这样交互自然很多：

👋
机器人，跟我。

10.2 Voice：语音控制状态机

当前：

ros2 service call ...

显然不适合最终产品。

可以加入 ASR：

“跟着我”
    ↓
START

“先停一下”
    ↓
PAUSE

“继续”
    ↓
RESUME

“不用跟了”
    ↓
FINISH

底层仍然调用：

/follow_me/state_control

这样 Voice 并不会破坏原有确定性控制接口。

10.3 VLM：让机器人知道“我要跟谁”

Re-ID 擅长：

这个人
是不是刚刚那个人

但不擅长：

找到穿红色衣服的人。

VLM 可以负责高层理解：

“跟着戴黄色安全帽的人。”
            ↓
VLM / Visual Grounding
            ↓
Target BBox
            ↓
Re-ID 初始化
            ↓
后续高速跟踪

正确分工应该是：

VLM
低频目标理解

Re-ID
中高频身份保持

Depth + PID
高频运动控制

而不是每一帧都：

VLM → cmd_vel

那样实时性和可控性都不理想。

10.4 Navigation：真正解决绕障跟随

当前 PID Follow Me：

Target
  ↓
Angle + Distance
  ↓
cmd_vel

升级后：

RGB-D
   ↓
人物相对坐标
   ↓
TF Transform
   ↓
Person Pose in map
   ↓
生成 Follow Goal
   ↓
Nav2
   ↓
Local Planner
   ↓
Collision Monitor
   ↓
Robot

此时：

桌子
椅子
行人
墙面

都可以交给 Navigation Stack 参与处理。

这才更接近真正产品级：

Person Following Robot。

11 常见问题 FAQ

Q1：为什么不用普通目标跟踪算法，非要 Re-ID？

普通 Tracker 很适合：

连续画面
短时无遮挡

但人物：

被遮挡
离开画面
重新出现
与其他人交叉

后，ID 很容易切换。

Re-ID 能利用人物外观 Feature 做身份恢复。

Q2：一定要 Depth Camera 吗？

按照当前实现：

是。

因为控制器直接需要：

current_distance

没有 Depth 就无法直接得到人与机器人的实际米制距离。

当然也可以以后改成：

Monocular Depth
Stereo
LiDAR + Camera Fusion

但就不是当前 Sample 的原始实现了。

Q3：为什么目标距离默认 2m？

它位于默认初始化范围：

1.5 ~ 3.0m

中间，是一个比较容易演示和控制的距离。

真实产品应该根据：

Camera FOV
机器人尺寸
使用场景
刹车距离
人员速度

重新确定。

Q4：为什么不用 Nav2？

这个 Sample 重点验证的是：

实时人物跟踪
+
视觉闭环运动控制

而不是地图导航。

如果用于真实商用机器人，则非常建议进一步与：

Nav2
Collision Monitor

结合。

Q5：为什么 person_class_id 是 0？

COCO 数据集中的：

person = class 0

如果你换了自己的模型：

person = 3

则对应参数必须调整。

Q6：为什么一开始机器人不跟？

按顺序查：

State 是否 START
        ↓
YOLO 是否检测到 Person
        ↓
人物是否在中央区域
        ↓
距离是否 1.5~3m
        ↓
中央 Candidate 是否只有一个
        ↓
ExtractFeature 是否成功

不要第一反应就调 PID。

Q7：机器人左右抖动怎么办？

优先：

降低 angular_kp

其次：

适当增加 angular_kd

同时建议增加：

3° 左右 Dead Zone

Q8：机器人一直前后震荡怎么办？

降低：

linear_kp

增加适量：

linear_kd

并给：

distance_error

增加约：

±5~10cm

死区。

12 结论

Follow Me 看起来只是一个：

“机器人跟着人走”

的小 Demo。

但真正把源码拆开，会发现它已经包含一条相当完整的机器人 AI 闭环：

RGB
   ↓
YOLO Person Detection
   ↓
Person BBox
   ↓
Re-ID
   ↓
Target Identity
   ↓
Depth
   ↓
Target Distance
   ↓
Camera Geometry
   ↓
Target Angle
   ↓
Dual PID
   ↓
cmd_vel
   ↓
AMR Motion
   ↓
下一帧重新感知

如果进一步从系统层级抽象：

感知
Perception
    ↓
识别
Identification
    ↓
定位
Localization
    ↓
跟踪
Tracking
    ↓
控制
Control
    ↓
执行
Actuation

这也是这类 Sample 最值得学习的地方。

真正做产品时，再继续向上叠加：

Gesture
+
Voice
+
VLM
+
Navigation
+
Collision Avoidance

就可以从一个单纯的：

Follow Me Demo

逐步发展成：

“跟着我”
“跟着穿红衣服的人”
“先停一下”
“绕过前面的桌子继续跟”
“跟我去会议室”

这样的自然交互机器人，而高通端侧平台在这里承担的角色，不只是简单的跑一个 YOLO 模型，而是同时承载：Vision+Re-ID+Sensor Processing+ROS+Robot Control形成一个真正运行在机器人本体上的端侧 AI 闭环。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

前端持续交付中的金丝雀全链路监控闭环：Prometheus、Jaeger 与飞书告警机器人

DAMO开发者矩阵

这家勇闯“生命禁区”的机器狗企业，拿下全球第一工业市场份额

而在这类需求中，最难啃的部分，是化工、电解铝等极端工业工况——高危、强磁、防爆要求严苛，属于人工难以到达、传统自动化方案难以覆盖的“生命禁区”，多数厂商选择绕行。对于行业级市场的下一程，IDC给出的建议是：选择擅长的细分场景做深做透，积累真实场景数据和任务经验，把“能巡检”升级为“能作业”，把“卖产品”升级为“卖解决方案”。智元机器人通过生态合作伙伴加速拓展工业应用，Boston Dynamics