机器狗自动跟随方案

视觉融合跟随方案说明jie_deamon 扩展设计方案对比、坐标系、方案1/2 详细实现、算力、与 robot_nexus 集成1. 现状与问题当前实现include/lidar_tracker.hppWeb 雷达俯视图双击 →setTarget(x, y)机器人坐标米每帧/scan在目标圆半径 0.3m内取雷达点 → 算质心 → 更新目标calculateFollowVelocity()根据目标位置算前后/左右/旋转速度APF 势场法用/scan做局部避障局限旁边多站一人时0.3m 圆内混入多人点云质心被拉向中间或点更多的人易跟错。算法不区分目标身份。改进思路相机负责认人谁雷达或深度负责测距多远最终仍写入同一个setTarget(x, y)后续跟随与避障逻辑可复用。2. 方案总览方案硬件认目标测距离算力适用场景当前2D 雷达圆内质心雷达极低单人、少遮挡、跟反射物A 雷达增强2D 雷达加权质心门控卡尔曼雷达极低小改代码轻微抗干扰方案 1RGB 2D 雷达bbox / track_idbbox 内雷达点中高已有普通相机不加深度方案 2RGB-Dbbox / track_id深度反投影中高有深度相机测距更稳D 标签UWB / AprilTag标签 ID标签位置低跟固定物购物车方案 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)三种来源写入格式相同来源(x, y) 怎么来当前纯雷达0.3m 圆内雷达点质心方案 1bbox 内雷达点质心方案 2深度反投影到 base 后取 x, y4.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. 方案 1RGB 相机 2D 雷达详细5.1 适用场景机器狗已有 2D 雷达 普通 USB/网络相机不想增加深度相机成本、重量、布线主要跟人走环境单人或旁人不会长时间贴在 0.5m 内onboard 或外接算力能跑YOLO 跟踪见第 8 节不适合没有相机标定条件雷达安装高度与人体完全扫不到算力极弱且无法 App offload。5.2 硬件与 ROS 话题项目说明输入/camera/image_raw,/camera/camera_info,/scan标定camera ↔ lidar或 camera ↔ base_link外参 相机内参 KTFbase_link→laser_link,base_link→camera_link输出/person_tracker/targetgeometry_msgs/PointStampedframebase_link5.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, z0) 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 做 APFcalculateFollowVelocity 不变5.4 投影公式雷达点 → 像素概念p_cam T_cam_lidar * p_lidar s * [u, v, 1]^T K * p_cam筛选条件in_bbox(u1uu2)and(v1vv2)# 建议用 bbox 下半部分v v1 0.5 * (v2 - v1)目标位置target_xmean(p.xforpincandidates)# base 系target_ymean(p.yforpincandidates)5.5 用户如何指定跟踪目标Web/App 显示相机 live 画面不是雷达俯视图用户点击要跟的人 →{ type: select_person, u: 640, v: 380 }视觉节点在本帧检测框中选包含该像素的框绑定track_id之后每帧只跟该track_id旁人进画面但不进同一 ID 不会影响可选 ROS Service/person_tracker/select_target(u, v) → track_id5.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或取时间戳最接近的一帧配对检测 510Hz、雷达 1020Hz 时允许 ±50ms 误差。5.8 优点与局限优点局限不需深度相机必须 camera–lidar 标定track_id 抗「旁人进圆」2D 雷达高度平面限制复用现有 APF 与跟随控制视觉检测占算力setTarget 接口不变bbox 内无雷达点时需降级5.9 异常与降级情况处理bbox 内 0 个雷达点保持上一帧 target Kalman 预测超时停车track 丢失提示用户重新点选检测帧率低跟踪器预测中间帧跟随 10Hz 仍可用6. 方案 2RGB-D 深度相机详细6.1 适用场景已有或可加RGB-DIntel RealSense、Orbbec 等希望距离测量更直接减少点云–图像匹配2D 雷达常扫不到人体但深度在 bbox 内仍有效同样需相机认人YOLO track_id不适合强逆光、玻璃、黑色吸光衣物导致深度大面积无效不愿做 camera–base 标定。6.2 硬件与 ROS 话题项目说明输入/camera/color/image,/camera/depth/image_rect_raw,/camera/camera_info或/camera/depth/color/points点云标定depth ↔ color 对齐驱动通常提供camera ↔ base_link 外参2D 雷达可选建议保留用于 APF 避障输出/person_tracker/targetbase_link 下 x, y6.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 的关键区别项目方案 1方案 2距离来源2D 雷达点深度图必须标定camera ↔ lidarcamera ↔ base雷达在测距中的作用必须可不参与测距雷达在避障中的作用测距 APF建议仍做 APF多人抗干扰track_id bbox 筛雷达点track_id bbox 取深度典型失败雷达扫不到腿深度空洞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_distcalculateFollowVelocity(target_x, target_y, ...)不变6.8 优点与局限优点局限不依赖 2D 雷达测距深度相机成本与功耗实现路径比投影匹配直观强光/玻璃/深色衣服 depth 差全身 bbox 底部 depth 较稳仍需视觉检测算力可与雷达 APF 并存需 depth–color 对齐6.9 异常与降级情况处理bbox 内有效 depth 10% 像素上一帧 Kalman或短时回退方案 1 雷达投影depth 全 0停车 提示track 丢失同方案 17. 方案 1 vs 2 选型已有 2D 雷达 普通相机无深度 → 方案 1 已有 / 可加 RGB-D → 方案 2人跟随通常更省心 跟固定购物车算力紧 → UWB / AprilTag非 1/2 仅小改、可接受多人干扰 → 方案 A纯雷达增强界面建议跟随时主界面用相机 live view 点人雷达俯视图仅作避障/调试不再用于选人。8. 如何传递「跟踪谁」的指令消息含义select_person(u, v)相机画面点人绑定 track_idswitch_track(track_id)换人stop_tracking停止视觉跟踪set_moving(enabled)与现有 jie_deamon 一致控制是否发布非零速度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_targetTopic/person_tracker/targetgeometry_msgs/PointStamped9. 算力要求引入相机后主要开销在人体检测YOLO不在setTarget或几何投影。配置检测约 FPS典型平台YOLOv8n CPU38树莓派 4、弱 ARMYOLOv8n NPU/GPU1530Jetson Orin Nano、RK3588纯雷达方案 A—零视觉算力降算力手段检测 5Hz DeepSORT 插帧YOLOv8n INT8 / TensorRT / RKNNROI 只在画面上半或上一帧 bbox 扩大区域检测App 侧检测机器狗只收 bbox track_id跟随控制 10Hz 足够检测510Hz多数场景可用。10. 实施步骤方案 1 / 2 通用评估算力 → 选 onboard 或 App offload标定外参方案 1 验证雷达点投影到图像对齐部署 YOLO DeepSORTperson_tracker节点Web 增加相机 live view 点选交互robot_nexus订阅 target关闭雷达质心更新保留 APF联调跟丢、depth 空洞、bbox 无点等降级策略11. 相关源码jie_deamon 现有文件作用include/lidar_tracker.hpp质心跟踪、APF、calculateFollowVelocityinclude/common_types.hppsetTarget、FOLLOW_DIST、TARGET_RADIUSinclude/kalman_filter.hpp可选目标平滑src/robot_nexus.cpp订阅/scan发布/cmd_velsrc/web_comm.cpp现有 WebSocket可扩展select_person