Kinect体感机器人（三）—— 空间向量法计算关节角度

By 马冬亮(凝霜  Loki)

一个人的战争(http://blog.csdn.net/MDL13412)

终于写到体感机器人的核心代码了，如何过滤、计算骨骼点是机器人控制的关键。经过摸索、评估、测试，最终得出了一个使用空间坐标进行计算的算法，下面我将进行详细讲解。

为什么是空间向量

说到角度计算，那么我们首先想到的就是解析几何，因为这是我们人类思考的方式。但是解析几何带来了一个问题——边界条件，使用其进行计算时，需要考虑各种特殊情况：平行、重叠、垂直、相交。。。这直接导致了代码量的爆炸性增长，而我们又知道，代码的BUG是与其长度呈指数级增长的，这给我们带来了沉重的心智负担，编码和调试都变得异常困难。

说到这，有人要说了，解析几何的边界条件无非就那么几种，我分模块进行编码就可以减少复杂度了，并不会损失太多。那么，请设想如下情况，如何计算手臂平面与地面的夹角？如下图：

空间解析几何带来了更多的边界条件，而Kinect在采集的过程中是不能下断点进行联机调试的，证明算法的正确性变得异常困难，这么多的边界条件，很难一一验证。

下面我们来看一组公式：

从上面这组公式可以看出，通过向量，我们可以完全摆脱边界条件的繁琐（对于骨骼点的重叠，可以通过滤波解决，见后文），只需编写通用的公式即可完成所有情况的计算，简单与优雅并存。

坐标映射

向量法使用的是常规的数学坐标系，而Kinect的坐标系与常规数学坐标系稍有不同，如下图所示：

由此可知，要使用向量，首先就要将Kinect的坐标系映射到数学坐标系上，其方法非常简单：由向量的可平移性质及方向性，可以推导出Kinect坐标系中任意两个不重合的坐标点A(x1, y1, z1)，B(x2, y2, z2)经过变换，可转化到数学坐标系中，对其组成的向量AB，可以认为是从坐标轴零点引出，转化公式如下：

根据上述性质，可以将人体关节角度计算简化为对空间向量夹角的计算。

空间向量法计算关节角度

由于所选用机器人的关节处舵机存在诸多限制，对于大臂保持静止，小臂与大臂垂直的旋转动作，需要借助于肩膀上的舵机进行联合调节。这就要求不能简单的只计算两空间向量的夹角，为此特提出了一种渐进算法，即求空间平面xOz与肩膀、肘关节、手所组成平面的夹角，并以其夹角完成对肩膀舵机的调速工作。

下面是实际人体左臂动作的计算过程，实拍人体动作照片见图A，左臂提取出的关节点在Kinect空间坐标系中的向量表示见图B，经过变换后转化为普通坐标系中的向量见图C。

图A 人体动作

图B 左臂关节点在Kinect困难关键坐标系中的向量表示

上图中：各个关节点（肩膀，肘，手）是处在空间平面中，对应z轴从里到外分别为：肩膀，肘，手，且三点在向量图中均处于z轴负半轴。

图C 经过变换后转化为数学坐标系中的向量

对于肘关节角度的计算，可以直接使用空间向量ES和EH的夹角得出，计算过程如下:

对于大臂的上下摆动角度，可以将向量ES投影到xOy平面上，并求其与y坐标轴的夹角得出，计算过程及公式类似于肘关节角度的计算过程。
        对于协助小臂转动的肩膀舵机的角度计算，其向量转化关系下所示：

为了求取空间平面夹角，需要首先求取两平面的法向量，再根据法向量计算出两平面夹角。计算过程如下：

式（3-5）和式（3-6）分别计算出向量ES和向量EH，分别对应肘关节指向肩膀和肘关节指向手腕的两条向量；式（3-7）通过叉乘计算出肩膀、肘、手所构成空间平面的法向量n1；式（3-8）代表空间平面xOz的法向量；式（3-9）求取法向量n1与法向量n2的夹角，从而完成对协助小臂转动的肩膀舵机的角度计算。
        对于腿部的识别，由于人体小腿无法旋转，故只需采用两向量夹角及投影到平面的方式进行求取，与手臂部分相似，不再详述。

腿部姿态检测

首先，由于机器人模仿人体腿部动作时会遇到平衡问题，为了解决此问题，需要给机器人加装陀螺仪和及加速度传感器，实时调整机器人重心，保持机器人站立的稳定性。但是在机器人调整稳定性同时，会导致机器人上肢的晃动，在机器人实际工作时，会造成手臂动作发生异常，可能导致意外发生。其次，机器人腿部动作大多局限于行走、转向、下蹲、站立等几个固定动作，让机器人完全模仿人体腿部动作，会给用户带来非常多的冗余操作，使用户不能专注于业务细节而需要专注于控制机器人腿部动作；最后，由于本文使用的人形机器人关节并不与人体关节一一对应，势必会造成控制上的误差，这可能带来灾难性的后果。
        综上所述，通过识别人体腿部特定动作，支持机器人前进、后退、左转、右转、下蹲、站立，即可满足绝大多数情况下对机器人腿部动作的要求，并且有效的减少了用户的操作复杂度，让用户可以专注于业务细节。
        为了支持机器人前进、后退、左转、右转、下蹲、站立这几个固定动作，需要对人体腿部姿态进行检测，从而控制机器人完成相应动作。检测算法首先检测左腿髋关节是否达到确认度阀值，若达到，则先检测是否为下蹲姿势，若不为下蹲，则检测左侧髋关节指向膝关节的向量相对于前、左、后三个方向哪个方向的权值更大，并取其权值最大的作为机器人控制信号，其分别对应与机器人的前进、左转、后退动作；若未达到阀值，则检测右髋关节是否达到确认度阀值，若达到，则检测右侧髋关节指向膝关节的向量相对于前、右、后三个方向哪个方向的权值更大，并取其权值最大的作为机器人控制信号，其分别对应与机器人的前进、右转、后退动作；若未达到阀值，则判定为机器人站立动作。
        腿部姿态详细检测流程如下图所示：

滤波算法

由于Kinect传感器采集到的数据会有扰动，从而造成机器人控制的不稳定性，因此必须对识别出来的骨骼点进行滤波处理，以保证机器人动作稳定、连贯。
        对于滤波算法的选择，要综合考虑运算速度、内存占用、准确性、抗随机干扰能力等技术指标。这就要求对采样数据进行分析，从而选取滤波效果最好的算法。
        本识别程序运行于EPCM-505C开发平台，在只进行关节识别的情况下，每秒能识别6-8帧图像，加上空间坐标向量运算及腿部姿势识别后，每秒能处理4-5帧图像。由于期望尽可能快的向机器人发送控制数据，以提高机器人的响应速度。因此，所选择的滤波算法应尽可能快速。
        经过对OpenNI识别出的关节点空间坐标分析可知，其扰动一般是在人体实际关节坐标的四周做小幅度波动，另外存在一些识别死区，此时无法检测到关节点。因此，所选用的滤波算法要保证机器人的正确运行，对无法识别的关节点做相应处理，对小幅度波动的关节点保持上一状态不变。
        综上所述，本文提出了一种改进型的限幅滤波算法，此滤波算法采用了动态规划的思想，保证每次滤波后的结果都是最优解，从而从整体上得出最优解。滤波算法的详细流程下图所示：

经过与常用滤波算法对比实验证明，此算法滤波效果良好，能满足对机器人控制的需求。详细对比结果如下表所示：

算法名称

技术指标

改进型限幅滤波算法

限幅滤波

算法

算术平均值

滤波算法

滑动平均值

滤波算法

速度

快

快

慢

较快

内存占用

少

少

多

多

能识别细微幅度动作

否

否

能

能

对小幅度扰动过滤效果

优

优

优

优

滤波结果是否正确

是

是

不一定

不一定
arccos哈系表

由于有16个关节角度需要计算，在PC上每秒可以运行30帧，即16 * 30 = 480次三角函数运算，这很明显是需要用打表进行优化的，下面是哈系表的代码，如果不明白，请绘制cos函数曲线，再进行对比阅读：

  1. /**
  2. * @brief 性能分析代码.
  3. * @ingroup ProgramHelper
  4. *
  5. * @details 用于分析查询哈希表和直接使用C库的三角函数计算角度值的性能.
  6. *
  7. * @code
  8. *
  9. #include <cstdlib>
  10. #include <iostream>
  11. #include <fstream>
  12. #include <cmath>
  13. #include <iomanip>
  14. #include <ctime>
  15. using namespace std;
  16. int main(int argc, char** argv)
  17. {
  18. clock_t start,finish;
  19. volatile double dummy;
  20. start=clock();
  21. // for (int i = 0; i < 1000; ++i)
  22. // for (int j = 0; j < 100000; ++j)
  23. // dummy = cos((i % 3) * M_PI / 180);
  24. for (int i = 0; i < 1000; ++i)
  25. for (int j = 0; j < 100000; ++j)
  26. dummy = (int)(((i % 3) * 1000)) % 100000;
  27. // for (int i = 0; i < 1000; ++i)
  28. // for (int j = 0; j < 100000; ++j)
  29. // dummy = i;
  30. finish = clock();
  31. cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;
  32. return 0;
  33. }
  34. *
  35. * @endcode
  36. */
  1. /**
  2. * @brief 生成cos哈希表的索引范围.
  3. * @ingroup ProgramHelper
  4. *
  5. * @details 将1-90度的cos值经过Hash函数变换, 得出一个哈希范围.
  6. * @see CosHashTable
  7. *
  8. * @code
  9. *
  10. #include <cstdlib>
  11. #include <iostream>
  12. #include <fstream>
  13. #include <cmath>
  14. #include <iomanip>
  15. using namespace std;
  16. int main(int argc, char** argv)
  17. {
  18. ofstream fout("a.txt");
  19. for (int i = 90; i > 0; --i)
  20. {
  21. fout << setw(6) << (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001);
  22. if (0 == i % 10)
  23. fout << "," << endl;
  24. else
  25. fout << ",";
  26. }
  27. fout << "100000";
  28. fout.flush();
  29. fout.close();
  30. return 0;
  31. }
  32. *
  33. * @endcode
  34. */
  35. static int s_initArccosHash[] =
  36. {
  37. 1745, 3489, 5233, 6975, 8715, 10452, 12186, 13917, 15643, 17364,
  38. 19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
  39. 35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
  40. 51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
  41. 65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
  42. 77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
  43. 87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
  44. 94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
  45. 98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
  46. };
  1. /**
  2. * @brief acrcos哈希表
  3. * @ingroup ProgramHelper
  4. *
  5. * @details 哈希函数:
  6. * 角度 = s_arccosHash[((int)(cos(degree) * 100000) % 100001]
  7. */
  8. static int s_arccosHash[100001];
关节角度计算核心代码

这里只给出左臂的关键代码，右臂及腿部类似，不再类述：

  1. // 计算机器人关节角度
  2. /**
  3. * @brief 计算机器人的左臂上3个舵机的角度.
  4. * @ingroup SceneDrawer
  5. *
  6. * @param shoulder 肩膀的坐标
  7. * @param elbow 肘关节的坐标
  8. * @param hand 手(腕)的坐标
  9. *
  10. * @details 所有坐标均采用向量法进行计算.
  11. */
  12. inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
  13. const XnSkeletonJointPosition &elbow,
  14. const XnSkeletonJointPosition &hand)
  15. {
  16. MyVector3D vector1;
  17. MyVector3D vector2;
  18. MyVector3D normal1;
  19. MyVector3D normal2;
  20. double deltaNormal1;
  21. double deltaNormal2;
  22. double deltaVector1;
  23. double deltaVector2;
  24. double cosAngle;
  25. if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
  26. {
  27. // vector1 -> shoulder - elbow
  28. // vector2 -> hand - elbow
  29. vector1.X = shoulder.position.X - elbow.position.X;
  30. vector1.Y = shoulder.position.Y - elbow.position.Y;
  31. vector1.Z = elbow.position.Z - shoulder.position.Z;
  32. vector2.X = hand.position.X - elbow.position.X;
  33. vector2.Y = hand.position.Y - elbow.position.Y;
  34. vector2.Z = elbow.position.Z - hand.position.Z;
  35. // normal1 = vector1 x vector2
  36. normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
  37. normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
  38. normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;
  39. normal2.X = 0.0;
  40. normal2.Y = -150.0;
  41. normal2.Z = 0.0;
  42. deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
  43. deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);
  44. if (deltaNormal1 * deltaNormal2 > 0.0)
  45. {
  46. cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);
  47. if (cosAngle < 0.0)
  48. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
  49. else
  50. {
  51. if (shoulder.position.Y < hand.position.Y)
  52. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
  53. else
  54. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];
  55. }
  56. }
  57. else
  58. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
  59. vector1.X = elbow.position.X - shoulder.position.X;
  60. vector1.Y = elbow.position.Y - shoulder.position.Y;
  61. vector1.Z = 0.0;
  62. vector2.X = 0.0;
  63. vector2.Y = 100;
  64. vector2.Z = 0.0;
  65. deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
  66. deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
  67. if (deltaVector1 * deltaVector2 > 0.0)
  68. {
  69. cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
  70. if (cosAngle < 0.0)
  71. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
  72. else
  73. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
  74. }
  75. else
  76. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
  77. vector1.X = shoulder.position.X - elbow.position.X;
  78. vector1.Y = shoulder.position.Y - elbow.position.Y;
  79. vector1.Z = elbow.position.Z - shoulder.position.Z;
  80. vector2.X = hand.position.X - elbow.position.X;
  81. vector2.Y = hand.position.Y - elbow.position.Y;
  82. vector2.Z = elbow.position.Z - hand.position.Z;
  83. deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
  84. deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
  85. if (deltaVector1 * deltaVector2 > 0.0)
  86. {
  87. cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
  88. if (cosAngle < 0.0)
  89. g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
  90. else
  91. g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
  92. }
  93. else
  94. g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
  95. } // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
  96. else
  97. {
  98. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
  99. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
  100. g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
  101. } // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
  102. #ifdef DEBUG_MSG_LEFT_ARM
  103. char bufferLeftArm[200];
  104. snprintf(bufferLeftArm, sizeof(bufferLeftArm),
  105. "LEFT_SHOULDER_VERTICAL = %4d LEFT_SHOULDER_HORIZEN = %4d LEFT_ELBOW = %4d",
  106. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],
  107. g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],
  108. g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
  109. std::cout << bufferLeftArm << std::endl;
  110. NsLog()->info(bufferLeftArm);
  111. #endif
  112. }
腿部姿态检测核心代码
  1. /**
  2. * @brief 判别机器人行走及下蹲.
  3. * @ingroup SceneDrawer
  4. *
  5. * @details 前后左右行走,下蹲.
  6. */
  7. inline void PoseDetect()
  8. {
  9. // 首先判断左腿
  10. if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
  11. {
  12. // 判断是否为蹲
  13. if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
  14. g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
  15. {
  16. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
  17. return;
  18. }
  19. // 需要判断向左踢腿的情况
  20. if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
  21. {
  22. // 判断是否达到向左踢腿的阀值
  23. if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
  24. {
  25. // 判断哪个方向的分量的权值更大
  26. if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  27. {
  28. if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
  29. abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
  30. {
  31. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  32. return;
  33. }
  34. else
  35. {
  36. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
  37. return;
  38. }
  39. }
  40. else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  41. {
  42. if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
  43. abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
  44. {
  45. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  46. return;
  47. }
  48. else
  49. {
  50. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
  51. return;
  52. }
  53. }
  54. else
  55. {
  56. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
  57. return;
  58. }
  59. }
  60. else
  61. {
  62. if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  63. {
  64. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  65. return;
  66. }
  67. else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  68. {
  69. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  70. return;
  71. }
  72. }
  73. }
  74. else // 直接判断是否是前进姿势
  75. {
  76. if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  77. {
  78. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  79. return;
  80. }
  81. else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  82. {
  83. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  84. return;
  85. }
  86. }
  87. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
  88. }
  89. if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
  90. {
  91. if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
  92. {
  93. if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
  94. {
  95. if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  96. {
  97. if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
  98. abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
  99. {
  100. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  101. return;
  102. }
  103. else
  104. {
  105. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
  106. return;
  107. }
  108. }
  109. else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  110. {
  111. if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
  112. abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
  113. {
  114. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  115. return;
  116. }
  117. else
  118. {
  119. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
  120. return;
  121. }
  122. }
  123. else
  124. {
  125. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
  126. return;
  127. }
  128. }
  129. else
  130. {
  131. if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  132. {
  133. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  134. return;
  135. }
  136. else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  137. {
  138. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  139. return;
  140. }
  141. }
  142. }
  143. else // 直接判断是否是前进姿势
  144. {
  145. if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
  146. {
  147. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
  148. return;
  149. }
  150. else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
  151. {
  152. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
  153. return;
  154. }
  155. }
  156. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
  157. }
  158. g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
  159. }
绘制人体骨骼核心代码
  1. /**
  2. * @brief 绘制骨骼图, 并返回相应的坐标点.
  3. * @ingroup SceneDrawer
  4. *
  5. * @param player 用户ID
  6. * @param eJoint1 第一个关节点ID
  7. * @param eJoint2 第二个关节点ID
  8. * @param joint1 [out] 关节点1
  9. * @param joint2 [out] 关节点2
  10. */
  11. inline void DrawLimbAndGetJoint(XnUserID player,
  12. XnSkeletonJoint eJoint1,
  13. XnSkeletonJoint eJoint2,
  14. XnSkeletonJointPosition &joint1,
  15. XnSkeletonJointPosition &joint2)
  16. {
  17. if (!TrackerViewer::getInstance().UserGenerator
  18. .GetSkeletonCap().IsTracking(player))
  19. {
  20. printf("not tracked!\n");
  21. return;
  22. }
  23. TrackerViewer::getInstance().UserGenerator
  24. .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
  25. TrackerViewer::getInstance().UserGenerator
  26. .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
  27. if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
  28. {
  29. return;
  30. }
  31. XnPoint3D pt[2];
  32. pt[0] = joint1.position;
  33. pt[1] = joint2.position;
  34. TrackerViewer::getInstance().DepthGenerator.
  35. ConvertRealWorldToProjective(2, pt, pt);
  36. glVertex3i(pt[0].X, pt[0].Y, 0);
  37. glVertex3i(pt[1].X, pt[1].Y, 0);
  38. }
  1. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
  2. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);
  3. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);
  4. s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
  5. s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
  6. s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;
  7. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
  8. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
  9. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);
  10. s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
  11. s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
  12. s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;
  13. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
  14. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
  15. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);
  16. s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
  17. s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
  18. s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;
  19. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
  20. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
  21. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);
  22. s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
  23. s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
  24. s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;
滤波算法核心代码
  1. static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];
  2. /**
  3. * @brief 对空间坐标点进行过滤.
  4. * @ingroup SceneDrawer
  5. *
  6. * @param id 关节ID
  7. * @param point [in out] 要过滤的点
  8. * @param filter 过滤阀值
  9. */
  10. inline void PointFilter(enum XnSkeletonJoint id,
  11. XnSkeletonJointPosition &point,
  12. const int filter)
  13. {
  14. if (point.fConfidence < JOINT_CONFIDENCE) // 过滤掉阀值以下的点
  15. {
  16. point = s_pointFilter[id];
  17. }
  18. else
  19. {
  20. if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
  21. {
  22. s_pointFilter[id] = point;
  23. }
  24. else
  25. {
  26. if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
  27. abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
  28. abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
  29. {
  30. point = s_pointFilter[id];
  31. }
  32. else
  33. {
  34. s_pointFilter[id] = point;
  35. }
  36. }
  37. }
  38. }

完整源码及论文

2015年9月6日更新下载链接：

论文

上位机源码

2017年5月8日更新下载链接

论文

上位机源码

下位机源码

Kinect体感机器人（三）—— 空间向量法计算关节角度相关推荐

  1. （转）Kinect体感机器人—— 空间向量法计算关节角度

    Kinect体感机器人(三)-- 空间向量法计算关节角度 By 马冬亮(凝霜  Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 终于写到体感机器人的核心代码了, ...

  2. Kinect体感机器人（二）—— 体感识别

    Kinect体感机器人(二)-- 体感识别 By 马冬亮(凝霜  Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 背景知识 体感技术属于NUI(自然人机界面)的 ...

  3. 用多个Kinect体感摄像头实现真正360度运动捕捉系统

    以前我在这里写过博客文章,研究用微软的Kinect体感摄像头做运动捕捉,当时设计了两种方案,一种是用NiTE中间件,在它的基础上改进了一点点,但处理360度转身主要靠插值,说白了就是靠猜测,效果不是很 ...

  4. 基于Kinect体感器控制的机械臂项目记录

    基于Kinect体感器控制的机械臂 项目介绍 上位机代码说明 1.识别部分 1.1GetPosition.cpp 1.2 KinetJiointFilter.cpp 1.3 1.4 SerialPor ...

  5. 基于Kinect体感的仿真对抗游戏系统

    项目意义: 1.Kinect的研究和应用在国内外都呈现出比较高的热潮,相关技术,如人脸识别.动态跟踪.手势识别等均出现了比较优越的实际效果.让机器人能够实时跟踪人的动作功能并完成与人的沟通互动是机器人 ...

  6. 制作Kinect体感控制小车教程 ＜一＞

    转载请注明出处:http://blog.csdn.net/lxk7280                                        Kinect体感控制小车        Kine ...

  7. 制作Kinect体感控制小车教程 lt;一gt;

    转载请注明出处:http://blog.csdn.net/lxk7280                                        Kinect体感控制小车        Kine ...

  8. Kinect体感互动解决方案——体感炫舞互动系统

    儿童乐园.商场人流较少.缺乏活力.死气沉沉? 可能你需要的是Kinect体感炫舞互动游戏. 佩京体感炫舞互动游戏,为儿童乐园.商场注入更多互动活力,您带来从未有过的超级酷炫体验,其精彩纷呈的动态舞台场 ...

  9. ces展,体感,机器人_2011年最佳CES（消费电子展）

    ces展,体感,机器人 This year, How-To Geek's own Justin was on-site at the Consumer Electronics Show in Las ...

最新文章

  1. 一口气说出 过滤器 和 拦截器 6个区别，别再傻傻分不清了
  2. 2020中国规模化敏捷大会-报名进行时
  3. percona-toolkit工具包安装
  4. RUNOOB python练习题9 如何在代码中加入砸瓦鲁多
  5. MySQL数据库select语句的使用方法
  6. css两张图片怎么合在一起_web前端入门到实战：纯CSS实现两个球相交的粘粘效果...
  7. 03 ansible核心模块 之 文件类型模块
  8. 从零打造 Vue 聊天组件
  9. zabbix监控添加主机，报警、监控的设置
  10. windows下MySQL 5.7+ 解压缩版安装配置方法
  11. java 等待时间_java工作复习——4大时间等待——显示等待
  12. 【Django 2021年最新版教程1】windows10+python3.9.5+pycharm2021.1.1+Django3.2.3新建一个web项目 教程
  13. 在Apache环境下成功的运行ASP.NET
  14. 关于vs08生成解决方案慢的解决方法
  15. 程序人生-Hello’s P2P
  16. 网易互娱的数据库选型和 TiDB 应用实践
  17. Netd 服务的 netd 套接字创建
  18. PHP支付宝转账到支付宝账号
  19. 空指针引用，导致linux内核panic（重启）
  20. uni-app分享小程序卡片给微信好友

热门文章

  1. MindSpore论坛活动——奖品免费领，祝大家开工大吉！
  2. Wrashall算法，自反性，对称性的实现
  3. 4399AT执行命令讲解
  4. 计算机c盘加容量,两种方法，给电脑C盘增加10G的容量，电脑焕然一新
  5. 1.调查问卷-接口文档
  6. platEMO：一款强大的多目标优化工具（MATLAB）
  7. 红包雨中：Redis 和 Lua 的邂逅
  8. 小丸子学Docker系列之——实战Dockerfile
  9. 2021-7-14-超融合基础知识
  10. TiDB上百T数据拆分实践