精华内容
下载资源
问答
  • fanuc机器人三点法工具坐标系设定

    千次阅读 2020-02-09 15:05:37
    机器人默认的工具坐标系原点(TCP)在J6轴的法兰盘中心的位置,为了方便法兰盘上其它工具的示教,所以在编程之前需要设定新的工具坐标系。未定义时,将由机械接口坐标系替代刀具坐标系。
    展开全文
  • FANUC机器人系统,介绍了用户坐标系的原理和使用方法,使用三点法设定机器人坐标系,详细介绍了设定的具体步骤和内容。
  • 本次以三点法示教的方式和大家分享简单的用户坐标系的设置方法。 具体步骤如下: 1.       按下MENU菜单键,显示出画面菜单。 2.     &nb

    FANUC机器人使用三点示教法设置用户坐标系的具体步骤
    首先要了解什么是用户坐标系?
    用户坐标系,是用户对每个作业空间进行定义的直角坐标系。在尚未被设定时,和世界坐标系是完全重合的,即:未设定用户坐标系时,0号用户坐标系和世界坐标系是完全重合的!
    本次以三点法示教的方式和大家分享简单的用户坐标系的设置方法。
    具体步骤如下:
    1.       按下MENU菜单键,显示出画面菜单。
    2.       选择设置setup
    3.       按下F1类型,显示出画面切换菜单
    4.       选择“坐标系”
    5.       按下F3“坐标”
    6.       选择“用户坐标系”,出现用户坐标系一览画面
    7.       将光标指向将要设定的用户坐标系号码所在行
    8.       按下F2详细,出现所选的坐标系号码的用户坐标系设定画面
    9.       按下F2方法
    10.    选择“3点记录”three point
    11.    如果要输入注释,将光标移动到注释行,输入注释
    12.    记录各参考点的方法:切换到世界坐标系,将光标移动到坐标原点,在点动方式下将机器人移动到要记录的点,在按住SHIFT键的同时,按下F5位置记录;将光标移动到X轴方向点,在点动方式下将机器人往需要设置的X轴方向移动大于250mm以上;在按住SHIFT键的同时,按下F5位置记录;将光标移动到Y轴方向点,在点动方式下将机器人往需要设置的Y轴方向移动大于250mm以上;在按住SHIFT键的同时,按下F5位置记录;将当前值的数据作为参考点输入。所示教的参考点,显示“记录完成”recorded

    展开全文
  • 三点法求曲率,使用C++实现

    千次阅读 2019-07-04 23:00:21
    三点法求曲率,使用C++实现 最近在做ROS路径规划的时候碰见了求路径斜率的问题,我采用了三点法求斜率,在这里记录一下。这是用C++写的,很好理解,易移植。 // An highlighted block try { //取点 P1、P2、P3是位置...

    三点法求曲率,使用C++实现


    最近在做ROS路径规划的时候碰见了求路径斜率的问题,我采用了三点法求斜率,在这里记录一下。这是用C++写的,很好理解,易移植。

    // An highlighted block
    try
    {   //取点 P1、P2、P3是位置信息
        P1 = PT1.pose.position;
        P2 = PT2.pose.position;
        P3 = PT3.pose.position;
        
        //计算曲率部分,使用正弦定理 a/sinA = 2R
        if(P1.x == P2.x == P3.x)//三点横坐标相同,即共线,直接标记曲率为0
        {
            curvity = 0;
        }
        else
        {
            double dis1,dis2,dis3;
            double cosA,sinA,dis;
            dis1 = sqrt((P1.x - P2.x)*(P1.x - P2.x) + (P1.y - P2.y)*(P1.y - P2.y));
            dis2 = sqrt((P1.x - P3.x)*(P1.x - P3.x) + (P1.y - P3.y)*(P1.y - P3.y));
            dis3 = sqrt((P2.x - P3.x)*(P2.x - P3.x) + (P2.y - P3.y)*(P2.y - P3.y));
            dis = dis1*dis1 + dis3*dis3 - dis2*dis2;
            cosA = dis/(2*dis1*dis3);//余弦定理求角度
            sinA = sqrt(1 - cosA*cosA);//求正弦
            curvity = 0.5*dis2/sinA;//正弦定理求外接圆半径
            curvity = 1/curvity;//半径的倒数是曲率,半径越小曲率越大
        }
        cout<<"curvity = "<<curvity<<"\n";
    }
    catch(tf::TransformException &ex)//处理意外情况
    {
        ROS_ERROR("%s",ex.what());
        cout<<"三点取点失败\n";
    }
    

    参考博客
    [1]: https://blog.csdn.net/Amych_/article/details/80209950

    展开全文
  • Kinect体感机器人(三)—— 空间向量计算关节角度 By 马冬亮(凝霜 &nbsp;Loki) 一个人的战争(http://blog.csdn.net/MDL13412) &nbsp; &nbsp; &nbsp; &nbsp; 终于写到体感机器人的核心代码...

    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.  
    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.  */  
    /**
     * @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 &lt; 1000; ++i)
        for (int j = 0; j &lt; 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 &lt;&lt; (double)(finish-start)/CLOCKS_PER_SEC &lt;&lt; endl;
    
    return 0;
    

    }
    *
    * @endcode
    */

    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. };  
    /* 
    * @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 &gt; 0; --i)
    {
        fout &lt;&lt; setw(6) &lt;&lt; (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001);
    
        if (0 == i % 10)
            fout &lt;&lt; "," &lt;&lt; endl;
        else
            fout &lt;&lt; ",";
    }
    
    fout &lt;&lt; "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
    };

    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];  
    / 
    * @brief acrcos哈希表
    * @ingroup ProgramHelper
    *
    * @details 哈希函数:
    * 角度 = s_arccosHash[((int)(cos(degree) * 100000) % 100001]
    */
    static int s_arccosHash[100001];

    关节角度计算核心代码

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

    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. }  
    
    // 计算机器人关节角度
    
    
    /**
     * @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 &lt;&lt; bufferLeftArm &lt;&lt; std::endl;
    NsLog()-&gt;info(bufferLeftArm);
    

    endif

    }

    腿部姿态检测核心代码
    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. }  
    /**
     * @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;
    }
    绘制人体骨骼核心代码
    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. }  
    /**
     * @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);
    }
    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;  
                    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;
    滤波算法核心代码
    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. }  
    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日更新下载链接

    论文

    上位机源码

    下位机源码

    展开全文
  • 机器人理论(3)DH表达:解析关节轴之间的关系

    万次阅读 多人点赞 2018-09-24 09:37:03
    导言 我们都知道关节一般会导致(驱动)机械臂产生两种...在这里需要借助DH表达来定义我们的坐标系的作法,并且给出需要求解的数值(平移/旋转)在空间上的表达。 本节是学习正逆向运动学的基础。 D-H表达(De...
  • 机器人工具坐标系标定

    千次阅读 2020-07-30 17:07:54
    4轴机器人标定TCP(两点法或三点法): 四轴机器人的TCP只有XY,没有其他四个轴。 两点法:根据两点的(X,Y,R)计算。根据两点的xy偏移与角度的偏移确定一个扇形的原点。 三点法:根据三点的(X,Y)计算。三点确定...
  • Kinect体感机器人(三)—— 空间向量计算关节角度 By 马冬亮(凝霜  Loki) 一个人的战争(http://blog.csdn.net/MDL13412)         终于写到体感机器人的核心代码了,如何过滤、计算...
  • 机器人走方格

    2019-06-27 20:22:34
    来源:牛客网 有一个XxY的网格,一个机器人只能走格点且只能向右或向下走,...请设计一个算法,计算机器人有多少种走。 给定两个正整数int x,int y,请返回机器人的走数目。保证x+y小于等于12。 测试样例: 2,2...
  • 一个机器人从坐标0,0的格子开始移动,每一次只能向左,右,上,下四个方向移动一格,但是不能进入行坐标和列坐标的数位之和大于threshold的格子。 例如,当threshold为18时,机器人能够进入方格[35,37],因为3+5+3+7...
  • 工业机器人典型控制系统及结构工业机器人由主体、驱动系统和控制系统三个基本部分组成。主体即机座和执行机构,包括臂部、腕部和手部,有的机器人还有行走机构。大多数工业机器人有3~6个运动自由度,其中腕部通常有...
  • 机器人定位误差标定模型

    千次阅读 2019-05-17 08:18:14
    新型产业环境下,工业机器人的应用范围将越来越广泛、作业任务也将越来越精细复杂,为满足现代制造技术及工艺的发展需求,工业机器人必须具备高精度、高柔性、自我维护和感应识别等特性。然而,现有机器人技术发展...
  • 针对目前自行走掘进机器人姿态测量设备只能依靠人工操作,无法连续、实时动态测量的缺点,基于三点法姿态测量,提出了一种多目标点的自动测量方法.该方法采用激光发射模块与接收模块,利用前方交会原理,通过测量多点...
  • 针对上述传统工业机械臂的三点缺陷,协作机器人在现有技术能达到的水平基础上开出了几张处方:第一是通过降低机械臂负载、限制运动速度、限制关节输出力矩、在机械臂上包裹软性材料、算法实现碰撞检测等方式,提高...
  • 工具坐标(tool):  使tcl坐标偏移到工具上,例如焊接工作,使... 通过三点法定义 x1,x2,y1  作用:定义工件坐标后,如更改机器人位置后,只需要重定义x1,x2,y1 不用更改程序之前定义的路径 有效载荷(lo...
  • 工业机器人常用的六种坐标系

    万次阅读 2019-11-28 11:07:52
    坐标系是为确定机器人的位置和姿态而在机器人或其他空间上设定的位姿指标系统。 工业机器人上的坐标系包括六种:大地坐标系(World Coordinate System)、基坐标系(Base Coordinate System)、关节坐标系(Joint ...
  • 三点定位在水下定位的用途,基于单片机,下载于万方
  • 本文已经首发在个人微信公众号:工业机器人仿真与编程(微信号:IndRobSim),欢迎关注! 概述 近几年来机器视觉技术在工业生产中得到了广泛的应用,在工业机器人应用领域中,机器视觉被广泛应用于工件的特征检测...
  • 工业机器人——工件定位问题

    千次阅读 2017-09-13 11:38:48
    需求分析:在工业机器人中进行工件的定位的时候,需要知道工件相对于机器人来说,在什么位置。首先,我们需要得到的就是通过雷达激光扫描或者其他方式得到的点云数据,然后对点云数据进行分析。 下面是我们目前得到...
  • 机器人坐标系相关基本知识

    千次阅读 2021-02-02 09:53:41
    机器人基本知识工业机器人的坐标系及TCP工业机器人轴的类型工业机器人坐标系类型关节坐标系直角坐标系世界坐标系工具坐标系工件坐标系用户坐标系坐标标定方法工具坐标系(没太明白)工件坐标系 工业机器人的坐标系及...
  • 机器人定位技术概述

    千次阅读 2017-08-01 10:22:54
    机器人导航的三个经典问题  说到机器人的自主导航,简单来说可以归结为由MIT教授JohnJ.Leonard和原悉尼大学教授Hugh Durrant-Whyte提出的三个问题:  (1)Where am I?  (2)Where I am going?  ...
  • 新建ABB机器人工程

    2021-01-05 14:11:24
    1、导入ABB机器人模型库。 2、导入机器人控制柜模型。 软件中控制柜只具备视觉意义,不具备实际的物理意义,所以,只做仿真的时候可以不导入。 3、快捷键: 平移 Ctrl+单击 缩放 滚轮 三维旋转 Ctrl+Shift+单击 4....
  • 扫地机器人常见的5种定位技术解析

    万次阅读 多人点赞 2017-04-12 14:32:29
    说到扫地机器人,想必大家都不陌生。它已成为众多家庭中的一位新成员,扫地机器人在早前就已兴起,近年来才逐渐普及开来,用过扫地机器人的朋友一定遇到过这样的情形,扫地机满地乱窜、杂乱无章,有些地方扫了好几遍...
  • 基于遗传算法的移动机器人路径规划

    万次阅读 多人点赞 2019-02-10 18:09:40
    之前在网上找基于遗传算法的移动机器人路径规划资料的时候,没有找到理想的开源代码。最后参照论文,用matlab写了代码。平时学习的时候,会看很多人写的一些博客和公众号文章,觉得很有帮助,所以也想分享一点自己学...
  • 自主定位导航是机器人实现智能化的前提之一,是赋予机器人感知和行动能力的关键因素。如果说机器人不会自主定位导航,不能对周围环境进行分析、判断和选择,规划路径,那么,这个机器人离智能还有一大截的差距。那么...
  • 工业机器人之工件标定

    千次阅读 2019-09-06 11:16:38
    在创建机器人离线编程时发现一问题,工件的标定不准确会导致离线编程系统生成的作业程序产生偏差。 1工件标定国内外现状 机器人离线编程,大部分的操作任务定义在工件坐标系下,因此对工件坐标系的准确标定对离线...
  • 机器人工具坐标系标定原理

    千次阅读 2020-11-05 16:16:58
    工业机器人使用过程中经常在机器人末端法兰面安装不同的工具来满足实际生产需求,为了准确控制工具运动的位置与姿态,需要对工具所在坐标系进行标定。 对于工业机器人来说,基坐标B与 末端法兰面所在坐标系 T之间的...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,042
精华内容 416
关键字:

机器人三点法