机器狗自动跟随方案

视觉融合跟随方案说明

jie_deamon 扩展设计：方案对比、坐标系、方案1/2 详细实现、算力、与 robot_nexus 集成

1. 现状与问题

当前实现（include/lidar_tracker.hpp）：

1. Web 雷达俯视图双击 →setTarget(x, y)（机器人坐标，米）
2. 每帧/scan在目标圆（半径 0.3m）内取雷达点 → 算质心 → 更新目标
3. calculateFollowVelocity()根据目标位置算前后/左右/旋转速度
4. APF 势场法用/scan做局部避障

局限：旁边多站一人时，0.3m 圆内混入多人点云，质心被拉向中间或点更多的人，易跟错。算法不区分目标身份。

改进思路：相机负责认人（谁），雷达或深度负责测距（多远），最终仍写入同一个setTarget(x, y)，后续跟随与避障逻辑可复用。

2. 方案总览

方案 1 与 2 的相同点：都在相机 live 画面里检测、跟踪人；用户点选画面指定目标；都用 DeepSORT 等维持track_id。

不同点：仅距离 / 3D 位置从哪来——方案 1 用 2D 雷达点投影进 bbox；方案 2 用深度图 bbox 内深度反投影。

3. 系统架构

┌─────────────┐ select_person(u,v) ┌──────────────────┐ │ Web / App │ ──────────────────────────► │ person_tracker │ │ 相机 live │ │ YOLO + DeepSORT │ └─────────────┘ └────────┬─────────┘ │ /camera/image │ 每帧输出 target(x,y) /camera_info │ in base_link /scan (方案1) ▼ /depth (方案2) ┌──────────────────┐ │ robot_nexus │ │ setTarget(x,y) │ │ lidar_tracker │ │ APF + /cmd_vel │ └──────────────────┘

分工：

• 相机驱动：只出图，不接收跟踪指令
• person_tracker 节点：接收「跟谁」，输出每帧(target_x, target_y)
• robot_nexus：消费setTarget，速度控制与 APF 可 largely 复用现有代码

4. setTarget 与坐标系

4.1 setTarget 存的是什么

SharedState::setTarget(x, y)表示目标在机器人本体坐标系下的位置：

• x：前方（米），与FOLLOW_DIST = 0.4比较决定进退
• y：左方（米），与atan2(y, x)决定转向

与lidar_tracker里雷达点转换后的(point_x, point_y)同一坐标系，不是：

• 雷达极坐标(range, angle)
• 图像像素(u, v)

三种来源，写入格式相同：

4.2 坐标系转换（通俗）

每个传感器有自己的「原点 + 前后左右」：

• base / 雷达系：以机器人为中心，x 前、y 左
• 相机系：以相机光心为中心，X Y Z
• 像素：u, v

转换= 已知相机相对机器人的安装位置（标定），把同一个物理点从一种说法换成另一种。

方案 1 方向：雷达点 → 相机系 → 投影到像素 → 判断是否在人的 bbox 内

方案 2 方向：像素 + 深度 → 相机系 3D → 变换到 base → 得到 (x, y)

相机系是连接「图像」与「机器人跟随控制」的桥梁。标定一次，参数固定（static_transform_publisher或 URDF）。

4.3 2D 雷达的高度限制

2D 雷达只有一个水平扫描面，常只扫到小腿；相机能看到全身。因此：

• bbox 宜取偏下（框底中心），提高与雷达平面相交概率
• 扫不到点时：Kalman 预测 / 保持上一帧 / 短暂停车提示重选

5. 方案 1：RGB 相机 + 2D 雷达（详细）

5.1 适用场景

• 机器狗已有 2D 雷达 + 普通 USB/网络相机
• 不想增加深度相机（成本、重量、布线）
• 主要跟人走，环境单人或旁人不会长时间贴在 0.5m 内
• onboard 或外接算力能跑YOLO + 跟踪（见第 8 节）

不适合：没有相机标定条件；雷达安装高度与人体完全扫不到；算力极弱且无法 App offload。

5.2 硬件与 ROS 话题

5.3 算法流程（每帧）

1. YOLO 检测所有人 → 多个 bbox 2. DeepSORT 分配/维持 track_id 3. 若用户已 select_person → 只处理 lock_track_id 对应 bbox 4. for each /scan 点: a. (range, angle) → lidar 系 (x, y, z=0) b. T_cam_lidar → 相机系 (X, Y, Z) c. K 投影 → 像素 (u, v)；Z≤0 丢弃 d. 若 (u,v) 在 lock bbox 内 → 加入候选集 5. 候选集算质心 (cx, cy)（base/lidar 系） 6. setTarget(cx, cy) 或发布 topic 给 robot_nexus 7. robot_nexus 仍用 /scan 做 APF，calculateFollowVelocity 不变

5.4 投影公式

雷达点 → 像素（概念）：

p_cam = T_cam_lidar * p_lidar s * [u, v, 1]^T = K * p_cam

筛选条件：

in_bbox=(u1<=u<=u2)and(v1<=v<=v2)# 建议用 bbox 下半部分：v >= v1 + 0.5 * (v2 - v1)

目标位置：

target_x=mean(p.xforpincandidates)# base 系target_y=mean(p.yforpincandidates)

5.5 用户如何指定跟踪目标

1. Web/App 显示相机 live 画面（不是雷达俯视图）
2. 用户点击要跟的人 →{ "type": "select_person", "u": 640, "v": 380 }
3. 视觉节点在本帧检测框中，选包含该像素的框，绑定track_id
4. 之后每帧只跟该track_id，旁人进画面但不进同一 ID 不会影响

可选 ROS Service：/person_tracker/select_target(u, v) → track_id

5.6 与 robot_nexus 集成

推荐：独立person_tracker节点，不塞进lidar_tracker.hpp。

// person_tracker 发布，robot_nexus 订阅voidonTarget(constgeometry_msgs::msg::PointStamped::SharedPtr msg){state_.setTarget(msg->point.x,msg->point.y);}

lidar_tracker::processScan中关闭或条件关闭原有圆内质心更新：

// 视觉模式下不再用雷达质心改 target，仅 APF + 速度计算if(!vision_target_active_){// 原有质心逻辑}

APF、走廊、calculateFollowVelocity、publishVelocity保持不变。

5.7 时间同步

图像与/scan时间戳对齐：

• ROS 2message_filters::ApproximateTime同步 image + scan
• 或取时间戳最接近的一帧配对

检测 5～10Hz、雷达 10～20Hz 时，允许 ±50ms 误差。

5.8 优点与局限

5.9 异常与降级

6. 方案 2：RGB-D 深度相机（详细）

6.1 适用场景

• 已有或可加RGB-D（Intel RealSense、Orbbec 等）
• 希望距离测量更直接，减少点云–图像匹配
• 2D 雷达常扫不到人体，但深度在 bbox 内仍有效
• 同样需相机认人（YOLO + track_id）

不适合：强逆光、玻璃、黑色吸光衣物导致深度大面积无效；不愿做 camera–base 标定。

6.2 硬件与 ROS 话题

6.3 算法流程（每帧）

1. YOLO 在彩色图检测人 → bbox + track_id（同方案 1） 2. 用户 select_person 锁定 track_id 3. 将 bbox 映射到对齐后的深度图 4. 在 bbox 内取有效深度（0.3m < d < 5m），取中值 d_med 5. 用 bbox 底中心 (u, v) + d_med + 内参 K 反投影: X = (u - cx) * d / fx Y = (v - cy) * d / fy Z = d 6. p_base = T_base_cam * (X, Y, Z) 7. setTarget(p_base.x, p_base.y) 8. /scan 仍供 lidar_tracker APF 使用

6.4 深度反投影公式

X = (u - cx) * d / fx Y = (v - cy) * d / fy Z = d [x, y, z, 1]^T = T_base_cam * [X, Y, Z, 1]^T target_x = x target_y = y

为何用 bbox 底中心：更接近脚/地面接触，与机器狗地面跟随一致。

为何用深度中值：抗噪、抗 bbox 边缘背景 depth。

6.5 与方案 1 的关键区别

6.6 用户指定目标（与方案 1 相同）

• 相机画面点选 →select_person(u, v)
• DeepSORT 锁定track_id
• 不是在深度图或雷达地图上单独点选

指令仍发给person_tracker，不是深度相机驱动。

6.7 与 robot_nexus 集成

与方案 1相同接口：

state_.setTarget(target_x,target_y);

lidar_tracker建议：

• vision_target_active_ == true时跳过雷达质心更新
• 继续processScan做 APF 与min_obstacle_dist
• calculateFollowVelocity(target_x, target_y, ...)不变

6.8 优点与局限

6.9 异常与降级

7. 方案 1 vs 2 选型

已有 2D 雷达 + 普通相机，无深度 → 方案 1 已有 / 可加 RGB-D → 方案 2（人跟随通常更省心） 跟固定购物车，算力紧 → UWB / AprilTag（非 1/2） 仅小改、可接受多人干扰 → 方案 A（纯雷达增强）

界面建议：跟随时主界面用相机 live view 点人；雷达俯视图仅作避障/调试，不再用于选人。

8. 如何传递「跟踪谁」的指令

WebSocket 扩展示例：

{"type":"select_person","u":640,"v":380}{"type":"track_update","track_id":7,"bbox":[300,100,400,500],"target":{"x":1.8,"y":0.2}}

ROS 2 建议：

• Service/person_tracker/select_target
• Topic/person_tracker/target（geometry_msgs/PointStamped）

9. 算力要求

引入相机后，主要开销在人体检测（YOLO），不在setTarget或几何投影。

降算力手段：

• 检测 5Hz + DeepSORT 插帧
• YOLOv8n INT8 / TensorRT / RKNN
• ROI 只在画面上半或上一帧 bbox 扩大区域检测
• App 侧检测，机器狗只收 bbox + track_id

跟随控制 10Hz 足够；检测5～10Hz多数场景可用。

10. 实施步骤（方案 1 / 2 通用）

1. 评估算力 → 选 onboard 或 App offload
2. 标定外参；方案 1 验证雷达点投影到图像对齐
3. 部署 YOLO + DeepSORT，person_tracker节点
4. Web 增加相机 live view + 点选交互
5. robot_nexus订阅 target，关闭雷达质心更新，保留 APF
6. 联调：跟丢、depth 空洞、bbox 无点等降级策略

11. 相关源码（jie_deamon 现有）