🤖 社恐福音！深大造出“跟屁虫”机器人：只要不叉腰，它就跟你到天荒地老！🚶‍♂️💨

image.png

> 来源：2024 年深圳大学暑期实训营

> 团队成员：秦阳、朱子恒、余希彤、张博焜、曹云笛

> 硬件平台：Orbbec Gemini 2 / Astra Pro + Jetson Nano + Orbbec S1 麦轮小车

> 技术栈：Astra Body Tracking SDK + ROS Melodic + OpenCV Haar + C++

> 源码传送门：GitHub 仓库

一、项目概述

本项目基于 RGB-D 深度相机与Astra Body Tracking SDK，实现了对人体19 个骨骼关节点的实时追踪，并在此基础上通过几何阈值判别法识别多种人体姿态（举手、伸臂、叉腰、抬脚、跌倒等），最终驱动 Orbbec S1 麦轮小车完成自主跟随与体感交互两大核心功能。系统同时集成OpenCV Haar 级联分类器实现人脸检测，并通过ROS 分布式消息机制将视觉感知、姿态决策、运动控制、语音反馈等模块松耦合连接，形成一套完整的智能机器人感知-决策-执行闭环。核心亮点：– 🦴19 点骨骼追踪：利用深度信息获取人体关节的 3D 世界坐标与 2D 深度坐标

– 🧠 零神经网络推理：纯几何关系判姿，算力开销极低，Jetson Nano 上稳定实时运行

– 🏎️ PD 控制跟随：基于质心偏移的比例-微分控制，跟随平滑无震荡

– 🎮 体感遥控：举手抬脚即可隔空操控小车前进后退、左右平移、旋转

– 🔊 语音反馈：姿态触发对应语音提示，增强交互体验

– 🔐 目标锁定机制：多人环境下锁定目标 ID，防止误触发

二、系统架构与 ROS 通信拓扑

2.1 节点通信拓扑

image.png

2.2 数据流说明

层级 节点 输入 Topic 输出 Topic 核心职责
感知层 main.cpp Astra SDK 底层数据流 /bodylist、/body/image、/body/face、/body/mask 相机驱动、骨骼追踪、人脸检测
决策层 bodydata_process.cpp /bodylist、/recoveryid /body_posture 姿态几何判别、目标锁定
控制层 follower.cpp /body_posture、/mode /cmd_vel PD 算法计算速度指令
交互层 interaction.cpp /body_posture、/mode /cmd_vel、/buzzer_flag 体感姿态 → 运动映射
反馈层 feedback.cpp /body_posture、/mode （调用 aplay） 姿态触发语音播报

三、感知层：Astra SDK 驱动与数据获取

3.1 骨骼关节点定义

Astra SDK 为每帧检测到的每个人体提供 19 个关节点，每个关节点包含世界坐标（worldPosition，单位 mm）和深度图像坐标（depthPosition，单位 pixel）。源码中通过宏定义建立索引映射：

image.png
// main.cpp 中定义的关节类型索引（对应 Astra SDK 的 19 个关节点）
#define HEAD            0
#define SHOULDER_SPINE  1   // 肩脊（颈部与脊柱连接处）
#define LEFT_SHOULDER   2
#define LEFT_ELBOW      3
#define LEFT_HAND       4
#define RIGHT_SHOULDER  5
#define RIGHT_ELBOW     6
#define RIGHT_HAND      7
#define MID_SPINE       8   // 脊柱中部
#define BASE_SPINE      9   // 脊柱基部（质心参考基准）
#define LEFT_HIP        10
#define LEFT_KNEE      11
#define LEFT_FOOT       12
#define RIGHT_HIP       13
#define RIGHT_KNEE     14
#define RIGHT_FOOT      15
#define LEFT_WRIST      16
#define RIGHT_WRIST     17
#define NECK            18
#define UNKNOWN         255

3.2 数据采集流程

main.cpp 通过 Astra C API 完成以下工作：

// 核心循环：每帧读取并发布数据
do {
    astra_update();                          // 更新 Astra 设备缓冲区
    astra_reader_open_frame(reader, 0, &frame);  // 获取一帧数据

    if (body_stream) {
        astra_frame_get_bodyframe(frame, &bodyFrame);
        output_bodyframe(bodyFrame);  // 输出骨骼 + 人体掩码
    }
    if (rgb_stream) {
        astra_frame_get_colorframe(frame, &colorFrame);
        print_color(colorFrame);    // 发布彩色图 + 人脸检测
    }
} while (shouldContinue);

关键设计点：–彩色图与人脸检测：print_color() 将 Astra 原始 RGB 数据转为 ROS sensor_msgs/Image 消息发布，同时调用 detect_and_publish_faces() 使用 Haar 级联分类器检测人脸并发布 /body/face 话题。

– 骨骼数据封装：output_bodies() 遍历 bodyList 中每个人体，将其 19 个关节点的 worldPosition 和 depthPosition 打包为自定义 ROS 消息 bodyreader::bodyList，供下游节点订阅。

– 人体掩码：output_body_mask() 提取 bodyMask 数据（每个像素标识属于哪个人体 ID），经降采样后发布 /body/mask。

四、决策层：姿态识别的几何阈值逻辑

这是整个系统算法核心所在。bodydata_process.cpp 订阅 /bodylist，对每一帧中每个人体的关节点坐标进行几何关系分析，通过预设的阈值判断当前姿态。

4.1 判别方法论

不同于深度学习方案（如 OpenPose、MediaPipe），本项目采用硬编码几何规则：

> 核心思想：利用人体各关节在世界坐标系中的相对位置关系（差值），构建一组if-then 判别规则。当特定关节间的坐标差值满足全部条件时，即判定为对应姿态。优势：无需 GPU 加速，CPU 即可实时运行，适合嵌入式平台部署。

image.png

4.2 完整姿态判别公式表

下表总结了 bodydata_process.cpp 中所有 8 种姿态的完整判别条件：

编号 姿态名称 触发条件（关节坐标不等式） 判定逻辑说明
① 左手平举(left_arm_out) `\ LEFT_SHOULDER.y – LEFT_ELBOW.y\ < 100 且 \ LEFT_ELBOW.y – LEFT_HAND.y\ < 150 且 LEFT_SHOULDER.x – LEFT_ELBOW.x > 200 且 LEFT_ELBOW.x – LEFT_HAND.x > 200 且 LEFT_HAND.y > 400` 肩-肘-手三点在Y 轴高度接近（水平），且X 轴依次向左递增，构成左臂水平伸展
② 右手平举(right_arm_out) `\ RIGHT_SHOULDER.y – RIGHT_ELBOW.y\ < 100 且 \ RIGHT_ELBOW.y – RIGHT_HAND.y\ < 150 且 RIGHT_ELBOW.x – RIGHT_SHOULDER.x > 200 且 RIGHT_HAND.x – RIGHT_ELBOW.x > 200 且 RIGHT_HAND.y > 400` 与左手对称，肩-肘-手Y 轴水平，X 轴依次向右递增
③ 左手上举(left_hand_raised) LEFT_HAND.y - LEFT_ELBOW.y > 180 且 LEFT_SHOULDER.y - LEFT_ELBOW.y > 150 且 LEFT_HAND.y > 400 手 > 肘 > 肩，Y 轴严格递增，构成左手向上举起
④ 右手上举 (right_hand_raised) RIGHT_HAND.y - RIGHT_ELBOW.y > 180 且 RIGHT_SHOULDER.y - RIGHT_ELBOW.y > 150 且 RIGHT_HAND.y > 400 与左手对称，右手向上举起
⑤ 叉腰 / 双臂张开(akimibo) LEFT_HAND.y - BASE_SPINE.y > 30 且 RIGHT_HAND.y - BASE_SPINE.y > 30 且 LEFT_SHOULDER.x - LEFT_ELBOW.x > 100 且 LEFT_HAND.x - LEFT_ELBOW.x > 100 且 RIGHT_ELBOW.x - RIGHT_SHOULDER.x < -100 且 RIGHT_HAND.x - RIGHT_ELBOW.x < -100 且 RIGHT_ELBOW.y - RIGHT_HAND.y > 50 且 LEFT_ELBOW.y - LEFT_HAND.y > 50 且 BASE_SPINE.y > 100 双手高于脊柱基部（手臂抬起），左臂向右伸展 + 右臂向左伸展（双臂张开呈倒 V 形），同时双手低于肘部（非上举）。这是条件最复杂的姿态，共 9 个不等式
⑥ 抬左脚(left_foot_up) LEFT_FOOT.y - RIGHT_FOOT.y > 120 且 BASE_SPINE.y - LEFT_FOOT.y > 200 且 BASE_SPINE.y - RIGHT_FOOT.y > 500 且 LEFT_FOOT.y > -800 且 RIGHT_FOOT.y > -800 左脚 Y 坐标显著高于右脚（左脚在前/上），且右脚远在脊柱基部下方（右脚支撑），构成左脚抬起
⑦ 抬右脚 (right_foot_up) RIGHT_FOOT.y - LEFT_FOOT.y > 120 且 BASE_SPINE.y - RIGHT_FOOT.y > 200 且 BASE_SPINE.y - LEFT_FOOT.y > 500 且 LEFT_FOOT.y > -800 且 RIGHT_FOOT.y > -800 与左脚对称，右脚抬起
⑧ 跌倒检测(fall) LEFT_KNEE.y - BASE_SPINE.y > -10 且 RIGHT_KNEE.y - BASE_SPINE.y > -10 且 BASE_SPINE.y < 0 双膝 Y 坐标接近或超过脊柱基部（人已躺倒），且脊柱基部 Y < 0（质心低于相机视野中线），触发跌倒报警

4.3 目标锁定机制

在多人体场景下，系统通过 lock_body_id 锁定目标：

static int lock_body_id = 0;  // 全局锁定目标 ID
int lock_status = 0;          // 0=无人  1=检测到人但未锁定  2=已锁定

// 叉腰姿态触发锁定（将当前人体 ID 设为锁定目标）
if (/* akimibo 条件满足 */) {
    lock_body_id = body.bodyid;
}

// 所有姿态判别仅在 body.bodyid == lock_body_id 时生效
if (body.bodyid == lock_body_id) {
    bodyposture_msg.left_arm_out = 1;  // 只有锁定的目标才触发
}

锁定状态机：

┌─────────┐   检测到人    ┌──────────┐   akimibo   ┌──────────┐
          │ 无人     │ ──────────▶ │ 检测到人  │ ──────────▶ │ 已锁定   │
          │(status=0)│ ◀────────── │(status=1)│ ◀────────── │(status=2)│
          └─────────┘  人离开消失   └──────────┘  重新叉腰    └──────────┘

此外，外部可通过 /recoveryid 话题手动重置锁定 ID，实现"重新选择跟踪目标"的功能。

4.4 模式切换逻辑

当人体做出 双手合抱 手势（双手均在脊柱两侧、深度值在合理范围内）时，触发模式切换：

if (LEFT_HAND.x - BASE_SPINE.x > 0 
    && BASE_SPINE.x - RIGHT_HAND.x > 0
    && BASE_SPINE.y > 0
    && LEFT_HAND.depthPosition.x > 140
    && LEFT_HAND.depthPosition.x < 500
    && RIGHT_HAND.depthPosition.x > 140
    && RIGHT_HAND.depthPosition.x < 500) {
    // 切换 mode: 1 ↔ 2
    mode_msg.data = (mode_msg.data == 1) ? 2 : 1;
    mode_Pub.publish(mode_msg);
    system("aplay ... mode_switch.wav");  // 播放模式切换提示音
}

这实现了 跟随模式 ↔ 体感交互模式 的无缝切换。

五、控制层：运动控制策略

5.1 PD 跟随控制（follower.cpp）

当 mode=2（跟随模式）时，follower.cpp 订阅 /body_posture 中的质心坐标 CenterOfMass_X 和 CenterOfMass_Z，通过 PD（比例-微分）控制器计算小车的线速度和角速度：控制方程：

$v = K_{p,x} cdot e_d + K_{d,x} cdot frac{de_d}{dt}$

$omega = K_{p,z} cdot e_theta + K_{d,z} cdot frac{de_theta}{dt}$

其中：

  • $e_d = Z_{actual} – Z_{target}$（距离误差，目标距离 2000mm）
  • $e_theta = arctan(X_{actual} / Z_{actual})$（角度误差）
  • $K_p$、$K_d$ 为可通过 ROS 参数服务器配置的比例/微分系数

死区与限幅：

// 死区：误差小于阈值时视为 0，防止微小抖动
if (error_x_angle > -0.01 && error_x_angle < 0.01) error_x_angle = 0;
if (error_distance > -80 && error_distance < 80) error_distance = 0;

// 限幅：线速度 [-0.5, 0.5] m/s，角速度 [-1, 1] rad/s
if (cmd_vel.linear.x < -0.5)  cmd_vel.linear.x = -0.5;
if (cmd_vel.angular.z < -1)   cmd_vel.angular.z = -1;

PD 控制的优势：D 项（微分项）对误差变化率敏感，能有效抑制小车在目标附近来回震荡，使跟随运动平滑自然。

5.2 体感交互控制（interaction.cpp）

当 mode=1（交互模式）时，小车不再自动跟随，而是将人体姿态直接映射为运动指令：

用户动作 小车响应 速度值
🦵 抬左脚 后退 linear.x = -0.15 m/s
🦵 抬右脚 前进 linear.x = 0.15 m/s
✋ 举左手 左转 angular.z = -0.2 rad/s
✋ 举右手 右转 angular.z = 0.2 rad/s
💪 伸左臂 左平移 linear.y = -0.1 m/s
💪 伸右臂 右平移 linear.y = 0.1 m/s
🚨 跌倒 蜂鸣器报警 buzzer_flag = 1

麦轮底盘支持全向移动（linear.y 即横向平移），这是差速底盘无法实现的。

六、反馈层：语音交互（feedback.cpp）

feedback.cpp 订阅 /body_posture 话题，根据识别到的姿态播放对应的语音文件，并内置防抖滤波：

// 抬脚动作需持续 5 帧以上才触发语音（防误触）
if (msg.left_foot_up == 1 && msg.right_foot_up == 0) {
    temp1++;
    if (temp1 > 5) {
        system("aplay ... backward.wav");
        temp1 = 0;
    }
}

语音映射表：

触发事件 音频文件 说明
叉腰 (akimibo) akimbo_cal.wav 标定完成提示
模式切换 mode_switch.wav 跟随↔交互切换提示
抬左脚 backward.wav 后退提示
抬右脚 forward.wav 前进提示
举左手 turn_left.wav 左转提示
举右手 turn_right.wav 右转提示
伸左臂 left_translation.wav 左平移提示
伸右臂 right_translation.wav 右平移提示
跌倒 fall.wav 跌倒报警
重新锁定 recovery.wav 目标重新锁定提示

七、人脸检测模块

系统同时维护了一套基于 OpenCV Haar 级联分类器的人脸检测流程：

– C++ 端（main.cpp）：加载 haarcascade_frontalface_default.xml，对 Astra 彩色图进行人脸检测，发布 /body/face 话题。

– Python 端（display.py）：订阅 USB 摄像头图像，使用 Haar 分类器检测人脸并绘制绿色边界框，发布 /body/body_display 话题供 rqt_image_view 实时查看。Haar + AdaBoost 原理简述：

Haar 特征是一种基于矩形区域的图像特征，通过计算白色和黑色矩形区域内像素灰度值之和的差值来表征人脸的局部特征（如眼睛、鼻梁、嘴巴）。AdaBoost（自适应增强）算法通过多轮训练，将多个弱分类器组合成一个强分类器，最终以级联（Cascade）方式快速排除非人脸区域，实现高效检测。

八、实验步骤与使用方法

8.1 环境准备

项目 要求
操作系统 Ubuntu 18.04
ROS 版本 Melodic
编译工具 Catkin
依赖库 Astra SDK、OpenCV 3.x、image_transport

8.2 启动流程

Step 1：启动 Astra 驱动与骨骼追踪

roslaunch bodyreader main.launch

Step 2：启动姿态识别与运动控制

# 终端 2：姿态识别决策节点
rosrun bodyreader bodydata_process

# 终端 3：PD 跟随控制节点
rosrun bodyreader follower

# 终端 4：体感交互控制节点
rosrun bodyreader interaction

# 终端 5：语音反馈节点
rosrun bodyreader feedback

Step 3：查看可视化画面

rqt_image_view
# 选择 /body/image 或 /body/body_display

8.3 操作指南

  1. 系统启动后，站在相机前，让小车检测到你（此时 lock_status=1）

2. 做出 叉腰（双臂张开） 手势 → 小车锁定你为目标（lock_status=2），开始跟随

3. 如需切换为体感遥控模式，再次做出叉腰手势即可

  1. 举手、抬脚等动作将触发对应的运动指令或语音反馈

九、参数调优指南

9.1 PD 控制参数

image.png
参数名 默认值 建议范围 作用
bodyfollow_x_p 0.01 0.005 ~ 0.05 距离比例系数，越大跟随越灵敏
bodyfollow_x_d 0.01 0.005 ~ 0.03 距离微分系数，越大阻尼越大、越平稳
bodyfollow_z_p 0.01 0.005 ~ 0.03 角度比例系数，越大转向越灵敏
bodyfollow_z_d 0.01 0.005 ~ 0.02 角度微分系数，抑制转向震荡

> 调参建议：先调 $K_p$ 使系统有响应，再调 $K_d$ 消除震荡。若出现"跟丢后冲出去"的现象，适当减小 $K_p$ 或增大死区阈值。

9.2 几何阈值调参建议

表中的阈值（如 200mm、150mm、100mm）是基于 S1 小车实际使用场景标定的。若更换相机安装高度或视角，建议：

– Y 轴阈值：与相机俯仰角强相关。相机俯视角度越大，Y 方向关节差值越小，需相应减小阈值

– X 轴阈值：与人体到相机的距离相关。距离越远，X 方向差值越小

– 建议方法：在目标场景中采集真实关节数据，用 printf 输出各关节坐标，根据实际分布重新标定

十、实验结果

系统经实测可稳定实现以下功能：

– ✅ 实时骨骼追踪：19 个关节点稳定追踪，帧率满足实时性要求

– ✅ 多姿态识别：左手/右手平举、上举、叉腰、抬脚、跌倒等 8 种姿态准确判别

– ✅ 平滑跟随：PD 控制下小车跟随流畅，无明显震荡

– ✅ 体感遥控：6 种体感指令响应及时，麦轮全向移动灵活

– ✅ 语音反馈：9 种事件触发对应语音，防抖逻辑有效避免误触发

– ✅ 目标锁定：多人环境下锁定可靠，叉腰重新标定流程顺畅

十一、应用场景参考

本项目的核心技术 —— RGB-D 人体姿态识别 + 自主跟随 + 体感交互，具备极强的通用性和扩展性，可迁移至以下典型应用场景：

11.1 🏠 智慧养老与独居监护

image.png

> 核心能力映射：跌倒检测 + 自主跟随 + 语音反馈

– 场景描述：在老龄化社会背景下，独居老人的安全问题日益突出。系统可持续监控老人的活动状态，当识别到跌倒姿态（双膝与脊柱基部 Y 坐标关系异常）时，立即触发本地蜂鸣器报警并播放 fall.wav 语音提示，同时可扩展接入通信模块自动拨打紧急联系人电话。

– 技术适配：利用本项目已有的 fall 姿态判别逻辑，配合距离感知能力，小车可始终与老人保持安全距离跟随，既能在老人行走时提供陪伴式跟随照明，又能在突发状况下第一时间响应。

– 扩展方向：接入 4G/WiFi 模块实现远程报警、结合心率传感器实现多模态健康监护。

11.2 🏭 智能工厂与仓储物流

image.png

> 核心能力映射：人体跟随 + 体感交互 + 目标锁定

– 场景描述：在柔性制造和智能仓储场景中，AGV（自动导引车）通常需要跟随工人完成物料转运。工人双手搬运货物时，无需操控遥控器，仅需正常行走，AGV 即可通过骨骼追踪自动跟随。当需要转向或停靠时，工人可通过简单的手势（举手、伸臂）下达指令。

– 技术适配：本项目的 mode=2 跟随模式天然适配物料跟随场景；mode=1 体感交互模式则可用于"手势指挥"场景，如仓库中指挥小车向左平移避障（linear.y）或紧急停止。

– 扩展方向：融合激光雷达 SLAM 实现自主避障导航、接入 WMS（仓储管理系统）实现任务调度。

11.3 🎯 安防巡检与可疑目标跟踪

image.png

> 核心能力映射：多目标识别 + 目标锁定 + 人脸检测

– 场景描述：在园区、机场、车站等安防场景中，监控机器人需要对特定人员进行持续跟踪。系统通过 lock_body_id 锁定目标后，即使环境中出现多人，也不会被非目标人员干扰。结合 Haar 人脸检测，可在跟踪过程中持续采集目标面部图像，供后端人脸识别系统比对。

– 技术适配：bodydata_process.cpp 中的目标锁定状态机（无人 → 检测到人 → 已锁定）确保了跟踪的连续性；recoveryid 话题支持安保人员手动切换跟踪目标。

– 扩展方向：接入边缘计算节点运行 YOLO 等深度学习检测模型，实现"人-物-行为"三位一体的智能安防分析。

11.4 🎮 展厅导览与互动娱乐

image.png

> 核心能力映射：体感遥控 + 语音反馈 + 全向移动

– 场景描述：在科技馆、企业展厅、商业综合体等场景中，机器人需要具备"吸睛"的交互能力。参观者可以通过举手、抬脚等自然肢体动作与机器人互动 —— 举左手让机器人左转介绍左侧展品，抬右脚让机器人前进引导路线。语音反馈模块则让交互更具趣味性和反馈感。

– 技术适配：麦轮底盘的全向移动能力（linear.y 横向平移）让机器人在狭窄的展厅通道中也能灵活走位；interaction.cpp 中 6 种姿态到运动指令的映射可直接用于导览场景。

– 扩展方向：结合 NLP（自然语言处理）实现语音对话导览、通过投影模块在地面投射互动光效。

11.5 🧪 科研教学与快速原型验证

image.png

> 核心能力映射：ROS 开源架构 + 模块化设计 + 低算力需求

– 场景描述：本项目的 ROS 分布式架构和模块化设计，使其成为高校机器人课程和实训营的理想教学平台。感知层、决策层、控制层、反馈层分工明确，学生可以在任一模块上进行替换和升级实验（例如将 Haar 替换为 YOLO，将几何判姿替换为 LSTM 动作分类器），而无需重构整个系统。

– 技术适配：Jetson Nano 的低功耗特性（5-10W）使得整套系统可以电池供电运行，非常适合课堂演示和实验室移动使用。

– 扩展方向：作为"机器人学""计算机视觉""人机交互"等课程的实验教具，支持多层次的进阶实验设计。

11.6 应用场景能力映射总览

应用场景 跌倒检测 自主跟随 体感交互 目标锁定 人脸检测 语音反馈
🏠 智慧养老 ✅ ✅ — ✅ ✅ ✅
🏭 仓储物流 — ✅ ✅ ✅ — —
🎯 安防巡检 — ✅ — ✅ ✅ —
🎮 展厅导览 — — ✅ — ✅ ✅
🧪 科研教学 ✅ ✅ ✅ ✅ ✅ ✅

> 💡 设计启示：上述场景的迁移并非简单的"换壳"，而是通过对 ROS 各功能节点的灵活组合与参数重配实现的。这种松耦合、可插拔的架构设计，正是 ROS 生态系统最大的工程价值所在。

十二、未来展望

1. 深度学习升级：将 Haar 级联分类器替换为 YOLOv8 或 MTCNN，提升遮挡和侧脸场景下的检测率

2. 卡尔曼滤波：在关节点追踪中引入 Kalman Filter，进一步平滑坐标抖动

3. 多目标切换：通过手势（如 pointing）实现"指谁跟谁"的自然交互

4. SLAM 融合：结合激光雷达 SLAM，在跟随过程中实现自主避障

5. 行为预测：基于历史轨迹预测目标运动趋势，提前规划跟随路径

📝 本文档由团队整理，基于 Orbbec Astra SDK 官方示例与实训营开发成果，适用于课程设计汇报、项目展示及技术文档归档。

发表回复

您的邮箱地址不会被公开。 必填项已用 * 标注