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

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函数曲线，再进行对比阅读：

[cpp]  view plain copy print ?

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.  
16. using namespace std; 
17.  
18. int main(int argc, char** argv) 
19. { 
20.     clock_t start,finish; 
21.     volatile double dummy; 
22.      
23.     start=clock(); 
24.      
25. //    for (int i = 0; i < 1000; ++i) 
26. //        for (int j = 0; j < 100000; ++j) 
27. //            dummy = cos((i % 3) * M_PI / 180); 
28.      
29.     for (int i = 0; i < 1000; ++i) 
30.         for (int j = 0; j < 100000; ++j) 
31.             dummy = (int)(((i % 3) * 1000)) % 100000; 
32.      
33. //    for (int i = 0; i < 1000; ++i) 
34. //        for (int j = 0; j < 100000; ++j) 
35. //            dummy = i; 
36.      
37.     finish = clock(); 
38.      
39.     cout << (double)(finish-start)/CLOCKS_PER_SEC << endl; 
40.      
41.     return 0; 
42. } 
43.  *  
44.  * @endcode 
45.  */  

[cpp]  view plain copy print ?

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.  
16. using namespace std; 
17.  
18. int main(int argc, char** argv) 
19. { 
20.     ofstream fout("a.txt"); 
21.      
22.     for (int i = 90; i > 0; --i) 
23.     { 
24.         fout << setw(6) << (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001); 
25.          
26.         if (0 == i % 10) 
27.             fout << "," << endl; 
28.         else 
29.             fout << ","; 
30.     } 
31.      
32.     fout << "100000"; 
33.      
34.     fout.flush(); 
35.     fout.close(); 
36.  
37.     return 0; 
38. } 
39.  *  
40.  * @endcode 
41.  */  
42. static int s_initArccosHash[] =  
43. {  
44.   1745,  3489,  5233,  6975,  8715, 10452, 12186, 13917, 15643, 17364,  
45.  19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,  
46.  35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,  
47.  51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,  
48.  65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,  
49.  77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,  
50.  87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,  
51.  94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,  
52.  98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000  
53. };  

[cpp]  view plain copy print ?

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];  

关节角度计算核心代码

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

[cpp]  view plain copy print ?

1.   
2. // 计算机器人关节角度  
3.   
4.   
5. /** 
6.  * @brief   计算机器人的左臂上3个舵机的角度. 
7.  * @ingroup SceneDrawer 
8.  *  
9.  * @param   shoulder 肩膀的坐标 
10.  * @param   elbow 肘关节的坐标 
11.  * @param   hand 手(腕)的坐标 
12.  *  
13.  * @details 所有坐标均采用向量法进行计算. 
14.  */  
15. inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,  
16.                              const XnSkeletonJointPosition &elbow,  
17.                              const XnSkeletonJointPosition &hand)  
18. {  
19.     MyVector3D vector1;  
20.     MyVector3D vector2;  
21.     MyVector3D normal1;  
22.     MyVector3D normal2;  
23.     double deltaNormal1;  
24.     double deltaNormal2;  
25.     double deltaVector1;  
26.     double deltaVector2;  
27.     double cosAngle;  
28.   
29.     if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)  
30.     {  
31.         // vector1      -> shoulder - elbow  
32.         // vector2      -> hand - elbow  
33.         vector1.X = shoulder.position.X - elbow.position.X;  
34.         vector1.Y = shoulder.position.Y - elbow.position.Y;  
35.         vector1.Z = elbow.position.Z - shoulder.position.Z;  
36.   
37.         vector2.X = hand.position.X - elbow.position.X;  
38.         vector2.Y = hand.position.Y - elbow.position.Y;  
39.         vector2.Z = elbow.position.Z - hand.position.Z;     
40.           
41.         // normal1 = vector1 x vector2  
42.           
43.         normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;  
44.         normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;  
45.         normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;  
46.           
47.         normal2.X = 0.0;  
48.         normal2.Y = -150.0;  
49.         normal2.Z = 0.0;          
50.           
51.         deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);  
52.         deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);  
53.           
54.         if (deltaNormal1 * deltaNormal2 > 0.0)  
55.         {  
56.             cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);  
57.           
58.             if (cosAngle < 0.0)  
59.                 g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;  
60.             else  
61.             {  
62.                 if (shoulder.position.Y < hand.position.Y)  
63.                     g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];  
64.                 else  
65.                     g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];          
66.             }   
67.         }  
68.         else  
69.             g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;  
70.           
71.         vector1.X = elbow.position.X - shoulder.position.X;  
72.         vector1.Y = elbow.position.Y - shoulder.position.Y;  
73.         vector1.Z = 0.0;  
74.   
75.         vector2.X = 0.0;  
76.         vector2.Y = 100;  
77.         vector2.Z = 0.0;  
78.           
79.         deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);  
80.         deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);  
81.   
82.         if (deltaVector1 * deltaVector2 > 0.0)  
83.         {  
84.             cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);  
85.   
86.             if (cosAngle < 0.0)  
87.                 g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);  
88.             else  
89.                 g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];  
90.         }  
91.         else  
92.             g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;  
93.   
94.         vector1.X = shoulder.position.X - elbow.position.X;  
95.         vector1.Y = shoulder.position.Y - elbow.position.Y;  
96.         vector1.Z = elbow.position.Z - shoulder.position.Z;  
97.   
98.         vector2.X = hand.position.X - elbow.position.X;  
99.         vector2.Y = hand.position.Y - elbow.position.Y;  
100.         vector2.Z = elbow.position.Z - hand.position.Z;     
101.   
102.         deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);  
103.         deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);  
104.   
105.         if (deltaVector1 * deltaVector2 > 0.0)  
106.         {  
107.             cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);  
108.   
109.             if (cosAngle < 0.0)  
110.                 g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);  
111.             else  
112.                 g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];  
113.         }  
114.         else  
115.             g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;  
116.     }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)  
117.     else  
118.     {  
119.         g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;  
120.         g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;  
121.         g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;  
122.     }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)  
123.                   
124. #ifdef DEBUG_MSG_LEFT_ARM  
125.     char bufferLeftArm[200];  
126.     snprintf(bufferLeftArm, sizeof(bufferLeftArm),   
127.              "LEFT_SHOULDER_VERTICAL = %4d  LEFT_SHOULDER_HORIZEN = %4d  LEFT_ELBOW = %4d",   
128.              g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],   
129.              g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],   
130.              g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);  
131.     std::cout << bufferLeftArm << std::endl;  
132.     NsLog()->info(bufferLeftArm);  
133. #endif  
134. }  

腿部姿态检测核心代码

[cpp]  view plain copy print ?

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.         // 需要判断向左踢腿的情况  
21.         if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])  
22.         {  
23.             // 判断是否达到向左踢腿的阀值  
24.             if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)  
25.             {  
26.                 // 判断哪个方向的分量的权值更大  
27.                 if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)  
28.                 {  
29.                     if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=   
30.                         abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))  
31.                     {  
32.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
33.                         return;  
34.                     }  
35.                     else  
36.                     {  
37.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;  
38.                         return;  
39.                     }  
40.                 }  
41.                 else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
42.                 {  
43.                     if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=  
44.                         abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))  
45.                     {  
46.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
47.                         return;  
48.                     }  
49.                     else  
50.                     {  
51.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;  
52.                         return;  
53.                     }  
54.                 }  
55.                 else  
56.                 {  
57.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;  
58.                     return;  
59.                 }  
60.             }  
61.             else  
62.             {  
63.                 if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)          
64.                 {  
65.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
66.                         return;  
67.                 }  
68.                 else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
69.                 {  
70.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
71.                         return;  
72.                 }  
73.             }  
74.         }  
75.         else    // 直接判断是否是前进姿势  
76.         {  
77.             if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)          
78.             {  
79.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
80.                     return;  
81.             }  
82.             else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
83.             {  
84.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
85.                     return;  
86.             }  
87.         }  
88.           
89.         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;  
90.     }   
91.       
92.   
93.   
94.     if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])  
95.     {  
96.         if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])  
97.         {  
98.             if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)  
99.             {  
100.                 if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)  
101.                 {  
102.                     if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=   
103.                         abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))  
104.                     {  
105.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
106.                         return;  
107.                     }  
108.                     else  
109.                     {  
110.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;  
111.                         return;  
112.                     }  
113.                 }  
114.                 else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
115.                 {  
116.                     if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=  
117.                         abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))  
118.                     {  
119.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
120.                         return;  
121.                     }  
122.                     else  
123.                     {  
124.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;  
125.                         return;  
126.                     }  
127.                 }  
128.                 else  
129.                 {  
130.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;  
131.                     return;  
132.                 }  
133.             }  
134.             else  
135.             {  
136.                 if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)          
137.                 {  
138.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
139.                         return;  
140.                 }  
141.                 else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
142.                 {  
143.                         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
144.                         return;  
145.                 }  
146.             }  
147.         }  
148.         else    // 直接判断是否是前进姿势  
149.         {  
150.             if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)          
151.             {  
152.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;  
153.                     return;  
154.             }  
155.             else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)  
156.             {  
157.                     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;  
158.                     return;  
159.             }  
160.         }  
161.           
162.         g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;  
163.     }   
164.       
165.     g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;  
166. }  

绘制人体骨骼核心代码

[cpp]  view plain copy print ?

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.   
24.     TrackerViewer::getInstance().UserGenerator  
25.         .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);  
26.     TrackerViewer::getInstance().UserGenerator  
27.         .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);  
28.   
29.     if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)  
30.     {  
31.         return;  
32.     }  
33.   
34.     XnPoint3D pt[2];  
35.     pt[0] = joint1.position;  
36.     pt[1] = joint2.position;  
37.   
38.     TrackerViewer::getInstance().DepthGenerator.  
39.         ConvertRealWorldToProjective(2, pt, pt);  
40.   
41.     glVertex3i(pt[0].X, pt[0].Y, 0);  
42.     glVertex3i(pt[1].X, pt[1].Y, 0);  
43. }  

[cpp]  view plain copy print ?

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.   
5. s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;  
6. s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;  
7. s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;  
8.   
9. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);  
10. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);  
11. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);  
12.   
13. s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;  
14. s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;  
15. s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;  
16.   
17. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);  
18. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);  
19. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);  
20.   
21. s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;  
22. s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;  
23. s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;  
24.   
25. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);  
26. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);  
27. DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);  
28.   
29. s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;  
30. s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;  
31. s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;  

滤波算法核心代码

[cpp]  view plain copy print ?

1. static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];  
2.   
3. /** 
4.  * @brief   对空间坐标点进行过滤. 
5.  * @ingroup SceneDrawer 
6.  *  
7.  * @param   id 关节ID 
8.  * @param   point [in out] 要过滤的点 
9.  * @param   filter 过滤阀值 
10.  */  
11. inline void PointFilter(enum XnSkeletonJoint id,   
12.                         XnSkeletonJointPosition &point,  
13.                         const int filter)  
14. {  
15.     if (point.fConfidence < JOINT_CONFIDENCE)   // 过滤掉阀值以下的点  
16.     {  
17.         point = s_pointFilter[id];  
18.     }  
19.     else  
20.     {  
21.         if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)  
22.         {  
23.             s_pointFilter[id] = point;  
24.         }  
25.         else  
26.         {  
27.             if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&  
28.                 abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&  
29.                 abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)  
30.             {  
31.                 point = s_pointFilter[id];  
32.             }  
33.             else  
34.             {  
35.                 s_pointFilter[id] = point;  
36.             }  
37.         }  
38.     }  
39. }  

完整源码及论文

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

论文

上位机源码