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函式曲線，再進行對比閱讀：
/**
* @brief 效能分析程式碼.
* @ingroup ProgramHelper
*
* @details 用於分析查詢雜湊表和直接使用C庫的三角函式計算角度值的效能.
*
* @code
*
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
#include <ctime>
using namespace std;
int main(int argc, char** argv)
{
clock_t start,finish;
volatile double dummy;
start=clock();
// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = cos((i % 3) * M_PI / 180);
for (int i = 0; i < 1000; ++i)
for (int j = 0; j < 100000; ++j)
dummy = (int)(((i % 3) * 1000)) % 100000;
// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = i;
finish = clock();
cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;
return 0;
}
*
* @endcode
*/
/**
* @brief 生成cos雜湊表的索引範圍.
* @ingroup ProgramHelper
*
* @details 將1-90度的cos值經過Hash函式變換, 得出一個雜湊範圍.
* @see CosHashTable
*
* @code
*
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
using namespace std;
int main(int argc, char** argv)
{
ofstream fout("a.txt");
for (int i = 90; i > 0; --i)
{
fout << setw(6) << (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001);
if (0 == i % 10)
fout << "," << endl;
else
fout << ",";
}
fout << "100000";
fout.flush();
fout.close();
return 0;
}
*
* @endcode
*/
static int s_initArccosHash[] =
{
1745, 3489, 5233, 6975, 8715, 10452, 12186, 13917, 15643, 17364,
19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
};/**
* @brief acrcos雜湊表
* @ingroup ProgramHelper
*
* @details 雜湊函式:
* 角度 = s_arccosHash[((int)(cos(degree) * 100000) % 100001]
*/
static int s_arccosHash[100001];關節角度計算核心程式碼
這裡只給出左臂的關鍵程式碼，右臂及腿部類似，不再類述：
////////////////////////////////////////////////////////////////////////////////
// 計算機器人關節角度
////////////////////////////////////////////////////////////////////////////////
/**
* @brief 計算機器人的左臂上3個舵機的角度.
* @ingroup SceneDrawer
*
* @param shoulder 肩膀的座標
* @param elbow 肘關節的座標
* @param hand 手(腕)的座標
*
* @details 所有座標均採用向量法進行計算.
*/
inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
const XnSkeletonJointPosition &elbow,
const XnSkeletonJointPosition &hand)
{
MyVector3D vector1;
MyVector3D vector2;
MyVector3D normal1;
MyVector3D normal2;
double deltaNormal1;
double deltaNormal2;
double deltaVector1;
double deltaVector2;
double cosAngle;
if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
{
// vector1 -> shoulder - elbow
// vector2 -> hand - elbow
vector1.X = shoulder.position.X - elbow.position.X;
vector1.Y = shoulder.position.Y - elbow.position.Y;
vector1.Z = elbow.position.Z - shoulder.position.Z;
vector2.X = hand.position.X - elbow.position.X;
vector2.Y = hand.position.Y - elbow.position.Y;
vector2.Z = elbow.position.Z - hand.position.Z;
// normal1 = vector1 x vector2
normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;
normal2.X = 0.0;
normal2.Y = -150.0;
normal2.Z = 0.0;
deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);
if (deltaNormal1 * deltaNormal2 > 0.0)
{
cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
else
{
if (shoulder.position.Y < hand.position.Y)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
vector1.X = elbow.position.X - shoulder.position.X;
vector1.Y = elbow.position.Y - shoulder.position.Y;
vector1.Z = 0.0;
vector2.X = 0.0;
vector2.Y = 100;
vector2.Z = 0.0;
deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
if (deltaVector1 * deltaVector2 > 0.0)
{
cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
vector1.X = shoulder.position.X - elbow.position.X;
vector1.Y = shoulder.position.Y - elbow.position.Y;
vector1.Z = elbow.position.Z - shoulder.position.Z;
vector2.X = hand.position.X - elbow.position.X;
vector2.Y = hand.position.Y - elbow.position.Y;
vector2.Z = elbow.position.Z - hand.position.Z;
deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
if (deltaVector1 * deltaVector2 > 0.0)
{
cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
} // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
else
{
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
} // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
#ifdef DEBUG_MSG_LEFT_ARM
char bufferLeftArm[200];
snprintf(bufferLeftArm, sizeof(bufferLeftArm),
"LEFT_SHOULDER_VERTICAL = %4d LEFT_SHOULDER_HORIZEN = %4d LEFT_ELBOW = %4d",
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
std::cout << bufferLeftArm << std::endl;
NsLog()->info(bufferLeftArm);
#endif
}腿部姿態檢測核心程式碼
/**
* @brief 判別機器人行走及下蹲.
* @ingroup SceneDrawer
*
* @details 前後左右行走,下蹲.
*/
inline void PoseDetect()
{
// 首先判斷左腿
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
{
// 判斷是否為蹲
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
return;
}
// 需要判斷向左踢腿的情況
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
{
// 判斷是否達到向左踢腿的閥值
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
{
// 判斷哪個方向的分量的權值更大
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else
{
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
}
else // 直接判斷是否是前進姿勢
{
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}
////////////////////////////////////////////////////////////////////////////////
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
{
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
}
else // 直接判斷是否是前進姿勢
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}繪製人體骨骼核心程式碼
/**
* @brief 繪製骨骼圖, 並返回相應的座標點.
* @ingroup SceneDrawer
*
* @param player 使用者ID
* @param eJoint1 第一個關節點ID
* @param eJoint2 第二個關節點ID
* @param joint1 [out] 關節點1
* @param joint2 [out] 關節點2
*/
inline void DrawLimbAndGetJoint(XnUserID player,
XnSkeletonJoint eJoint1,
XnSkeletonJoint eJoint2,
XnSkeletonJointPosition &joint1,
XnSkeletonJointPosition &joint2)
{
if (!TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
TrackerViewer::getInstance().DepthGenerator.
ConvertRealWorldToProjective(2, pt, pt);
glVertex3i(pt[0].X, pt[0].Y, 0);
glVertex3i(pt[1].X, pt[1].Y, 0);
} DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);
s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);
s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);
s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);
s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;濾波演算法核心程式碼
static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];
/**
* @brief 對空間座標點進行過濾.
* @ingroup SceneDrawer
*
* @param id 關節ID
* @param point [in out] 要過濾的點
* @param filter 過濾閥值
*/
inline void PointFilter(enum XnSkeletonJoint id,
XnSkeletonJointPosition &point,
const int filter)
{
if (point.fConfidence < JOINT_CONFIDENCE) // 過濾掉閥值以下的點
{
point = s_pointFilter[id];
}
else
{
if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
{
s_pointFilter[id] = point;
}
else
{
if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
{
point = s_pointFilter[id];
}
else
{
s_pointFilter[id] = point;
}
}
}
}完整原始碼及論文
2015年9月6日更新下載連結：
2017年5月8日更新下載連結
相關文章
- Kinect體感機器人（一）—— 整體設計機器人
- Kinect體感機器人（二）—— 體感識別機器人
- OpenStack 計算節點關機，虛擬機器狀態解決辦法虛擬機
- 預言機節點需衡量節點伺服器安全性儲存空間和計算能力伺服器
- 從作業系統角度看錶空間計算方式作業系統
- PCL程式設計-法向量計算程式設計
- Kinect體感互動解決方案——體感漫畫拍照遊戲遊戲
- 文字向量空間模型模型
- 空間資料庫三維空間兩點距離計算錯誤資料庫
- 空間距離計算
- 笛卡爾空間力反饋的關節速度控制之機器人運動學庫KDL安裝及使用機器人
- 關於計算時間複雜度和空間複雜度時間複雜度
- 【財富空間】計算機起源的數學思想計算機
- 一個讓人感傷又溫馨的計算機故事計算機
- 【scipy 基礎】--空間計算
- 三維空間中物體兩次旋轉之間的連續旋轉矩陣計算矩陣
- 剛體在三維空間的旋轉(關於旋轉矩陣、DCM、旋轉向量、四元數、尤拉角)矩陣
- 加入視覺：將計算機改造為機器人視覺計算機機器人
- 【機器視覺與影象處理】基於MATLAB的角度計算視覺Matlab
- 機器人與觸覺感測技術的碰撞，一文初探人類與機器人的觸覺感測機器人
- C++中struct的空間計算C++Struct
- 地理空間距離計算優化優化
- 人腦與計算機計算機
- 微軟CEO：聊天機器人將顛覆人們對計算機的使用微軟機器人計算機
- INDEMIND：多感測器融合，機器人的必由之路機器人
- 節卡機器人硬核實力實現“小巨人”三級跳機器人
- 打過球的機器人來了 網紅機器人空降啤酒節機器人
- python ubuntu lib7 -計算人臉特徵向量PythonUbuntu特徵
- HoloLens試用體驗：人與計算機關係因此改變計算機
- Java虛擬機器的記憶體空間有幾種Java虛擬機記憶體
- Java虛擬機器的記憶體空間有幾種！Java虛擬機記憶體
- 向量點積計算javaJava
- 09:向量點積計算
- 如何計算Java物件佔用了多少空間？Java物件
- 對oracle資料表空間的計算Oracle
- 計算表空間使用率指令碼指令碼
- 給虛擬機器的增加空間虛擬機
- Virtualbox 《虛擬機器空間整理》虛擬機