一种仿人机械臂体感控制系统及控制方法与流程

本发明属于机器人控制领域，更具体地，涉及一种仿人机械臂体感控制系统及控制方法。
背景技术：
：多数交互设备需要操作者掌握专业知识才能进行相应的交互操作。而体感技术降低了操作者对外设专业知识的掌握，简化了学习和训练的过程，依靠实时肢体(手势)识别、面部识别及语音识别就可以实现对机器的直接控制。用这种最自然的方式与机器进行交互，可以减轻操作者的负荷。自然的人机交互方式，越来越受到重视。通常，基于体感设备的控制方法采用可穿戴设备来采集骨骼信息，这种设备穿戴繁琐，降低了体感控制的可操作性。目前获取的人体骨骼信息后，大多通过空间向量法计算人体关节角，直接映射到机器人关节角用于控制机器人。但是这种控制方法，由于获取关节数量较少，计算自由度多，操作复杂，控制不精确，缺乏对机器人的完整描述。技术实现要素：针对现有技术的上述问题，本发明旨在克服传统机械臂控制方法操作复杂的缺陷，减少控制机器人所需计算和输出的自由度，使机器人控制更加智能、准确和快速。为了实现上述目的，本发明提供了一种仿人机械臂体感控制系统，包括：体感检测设备、机械臂控制器、机械臂和数据交互平台；体感检测设备的输出端连接数据交互平台的输入端，数据交互平台与机械臂控制器双向连接，机械臂控制器的输出端连接机械臂；体感检测设备，用于提取人体的肩关节中点B、肩关节位置S、肘关节位置E、腕关节位置W、手掌位置H、手末端位置HT和拇指位置T；机械臂，具有对应模拟人体的肩关节中点B'、肩关节位置S'、肘关节位置E'、腕关节位置W'、手掌位置H'；1号关节位于B'和S'之间，1号连杆连接1号关节和S'，2号连杆连接S'和3号关节，3号连杆连接3号关节和E'，4号连杆连接E'和5号关节，5号连杆连接5号关节和W'，6号连杆连接W'和7号关节，7号连杆连接7号关节和H'；S'、E'、W'分别是2号、4号、6号关节；数据交互平台，用于接收和处理体感检测设备上传的人体各位置数据，并据此计算出人体臂型角ψ、手掌位置BPH、手掌姿态BRH，从而根据ψ、BPH、BRH结合机械臂的运动参数计算出机械臂的角度θi，θi表示i号连杆绕i号关节轴线的转角，i＝1,2,…,7；数据交互平台还用于将θi发送给机械臂控制器；机械臂控制器用于将θi转换成运动指令以控制机械臂的运动并将机械臂的运动信息反馈给数据交互平台。进一步地，还包括虚拟场景显示控制台，虚拟场景显示控制台连接在数据交互平台和体感检测设备之间，用于模拟机械臂所处环境以及机械臂的运行结果并根据模拟结果决定机械臂是否执行数据交互平台的运动指令。进一步地，数据交互平台用于按照如下方式确定臂型角ψ：将B、S、W三点确定的平面作为参考平面，将S、W、E三点确定的平面作为臂平面，计算参考平面和臂平面的夹角，记为臂型角ψ。进一步地，数据交互平台用于按照如下方式确定手掌位置BPH：根据B点和H点的坐标，得出手掌相对肩关节中点的位置BPH，BPH＝[(xH-xB),(yH-yB),(zH-zB)]其中，xH、yH、zH是H点的坐标，xB、yB、zB是B点的坐标。进一步地，数据交互平台用于按照如下方式确定手掌位置BPH：根据腕关节W和手掌位置H的坐标，求得向量该方向记为ZH；手掌点H、拇指点T和手掌末端HT三点构成一个平面，XH垂直于该平面；YH是根据ZH和XH由笛卡尔坐标系确定；手臂末端的姿态描述为BRH＝[XH,YH,ZH]。为了实现上述目的，本发明还提供了一种用于上述仿人机械臂体感控制系统的控制方法，包括如下步骤：(1)人体臂型角ψ、手掌位置BPH、手掌姿态BRH的读取及确认体感检测设备提取提取人体上B、S、E、W、H、HT和T七个位置并上传给数据交互平台，在数据交互平台中进行如下定义和计算：将B、S、W三点确定的平面作为参考平面，将S、W、E三点确定的平面作为臂平面，计算参考平面和臂平面的夹角，记为臂型角ψ；根据B点和H点的坐标，得出手掌相对肩关节中点的位置BPH，BPH＝[(xH-xB),(yH-yB),(zH-zB)]其中，xH、yH、zH是H点的坐标，xB、yB、zB是B点的坐标；根据腕关节W和手掌位置H的坐标，求得向量该方向记为ZH；手掌点H、拇指点T和手掌末端HT三点构成一个平面，XH垂直于该平面；YH是根据ZH和XH由笛卡尔坐标系确定；手臂末端的姿态描述为BRH＝[XH,YH,ZH]。(2)数据交互平台根据人体臂型角ψ、手掌位置BPH、手掌姿态BRH结合机械臂的运动学参数计算机械臂的角度θi，θi表示i号连杆绕i号关节轴线的转角，i＝1,2,…,7；(3)数据交互平台将θi输入到机械臂控制器，并由机械臂控制器控制机械臂运动，i＝1,2,…,7。进一步地，还包括虚拟场景显示控制台，在步骤(2)和步骤(3)之间增加如下判定步骤：在虚拟场景显示控制台中模拟机械臂所处环境，以及根据步骤(2)中得到的θi模拟机械臂的运行结果，i＝1,2,…,7，如果模拟结果为机械臂运行有障碍，则虚拟场景显示控制台反馈障碍信息，提示捕捉对象变更动作，返回步骤(1)重新从步骤(1)开始执行；如果模拟结果为机械臂正常运行，则执行步骤(3)。进一步地，步骤(2)中，θi的获得方法如下：(a)坐标系的建立以i号关节+1的轴线为Zi轴，平行于i号关节的轴线的方向为Yi轴建立关于i号连杆的i号坐标系，i＝1,2,…,6；以B'为原点，1号关节的轴线为Z0轴，平行于1号关节的轴线的方向为Y0轴建立0号坐标系作为基坐标系；以H'为原点，关节7的轴线为Z0轴，平行于关节6的轴线的方向为Y7轴建立坐标系7作为机械臂末端坐标系；Xi是根据Zi和Yi由右手定则确定，i＝0,1,…,7；(b)根据捕捉得到的人体手掌位置BPH、人体手掌姿态BRH得到基坐标系中机械臂末端位置0P7和机械臂末端姿态0R7；由人体臂型角ψ得到机械臂的臂型角ψ′，ψ′＝ψ；(c)θ4求解S'、E'、W'在初始状态下的位置用S0'、E0'和W0'表示，与的夹角即为肘部关节角θ4；由于θ4在机械臂的臂平面上，θ4与ψ的大小无关：θ4=θ40-θ4t---(2)]]>其中，表示初始状态下和的夹角，表示在t时刻时和的夹角；(d)θ1、θ2和θ3求解3号坐标系相对于基坐标系的姿态0R3的第一个表达式如下：R03=R0ψ·R3ψ=00---(13)]]>其中，0Rψ-机械臂的臂平面相对于机械臂的参考平面的空间向量关系的旋转矩阵；-当3号坐标系的X3O3Y3平面与参考平面重合时，3号坐标系相对于基座标系的旋转矩阵；而根据正运动学方程可得0R3用θ1、θ2和θ3来表达的第二个表达式如下：R03=s1s3+c1c2c3-c1s2s1c3-c1c2s3-c1s3+s1c2c3-s1s2c1c3-s1c2s3c3s2c2-s2s3---(14)]]>式中，si＝sinθi，ci＝cosθi，i＝1、2、3；方程(13)(14)右侧的对应项相等，求出θ1、θ2和θ3；(e)θ5、θ6和θ7求解；4号坐标系相对于7号坐标系的姿态7R4有如下两种表达式：R74=R7T0·R0ψ·R3ψ=00·R34---(15)]]>R74=c5c6c7-s5s7-c5c6s7-s5c7c5s6s5c6c7+c5s7-s5c6s7-c5c7s5s6-s6c7s6s7c6---(16)]]>式中，si＝sinθi，ci＝cosθi，i＝5、6、7；-机械臂的末端位置0P7的转置矩阵；3R4-4号坐标系相对于3号坐标系的旋转矩阵；方程(15)(16)右侧的对应项相等，求出θ5、θ6和θ7。本发明提出的体感控制仿人机械臂的控制系统和控制方法与现有技术相比，具有以下优点：基于手臂臂型角的仿人机械臂体感的控制方法，将人体手臂的位置和姿态进行了完整的定义，避免了由于体感设备获取的骨骼数据不足，仿人机械臂自由度较多，机械臂控制不精确的缺陷。附图说明图1是体感设备获取的人体骨骼位置点；图2是人体骨骼信息基坐标系示意图；图3是人体手臂臂型角的定义示意图；图4是人体手掌坐标系的定义示意图；图5是S-R-S仿人机械臂构型及7个自由度示意图；图6是机械臂臂型角定义示意图；图7是机械臂DH坐标系示意图；图8是机械臂的臂型角计算示意图；图9是仿人机械臂体感控制系统示意图；图10是仿人机械臂控制流程示意图。具体实施方式为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。1、人体骨骼信息的获取人体骨骼信息的获取方式通常为基于视觉识别测量或者通过佩戴测量节点来实现。KinectV2是微软开发的新一代3D体感摄像机，用于体感游戏和科研人员开发。KinectSDK包含两个重要的API，分别是NUIAPI(自然用户界面)和AudioAPI。UNIAPI是KinectAPI的核心，实现从图像传感器中提取数据并控制Kinect设备。Kinect通过红外线发生装置发射红外线，并探测红外光反射，从而可以计算出视场范围内每一个像素的深度值，即获得深度数据，从深度数据中最先提取出来的是物体主体和形状。然后用这些形状信息来匹配人体的各个部分，最后计算匹配出来的各个关节在人体中的位置，Kinect获取的人体骨骼信息如图1所示。这些关节点的三维坐标可以通过Skeleton的Joints属性得到。通过Joints属性获得的关节三维坐标，坐标系方向如图2所示，Z向表示Kinect传感器的方向。在控制机械臂运动过程中，只提取主要的7个关节信息，分别是肩关节中点B，肩关节位置S，肘关节位置E，腕关节位置W，手掌位置H，手末端位置HT和拇指位置T。2、臂型角参数化的人体骨骼信息人体在自然状态下的一种姿态如图3所示。图中点B，S，E，W和H分别表示颈部，肩关节，肘关节，腕关节和手掌的位置。从当前的姿态可以发现，肩关节S、肘关节E和腕关节W可以构成一个平面。人体手臂在运动过程中，该平面会随着手臂姿态变化。通过描述该平面的位置，结合手掌的位置，可以准确的表达手臂的运动状态。为描述肩关节、肘关节和腕关节构成的平面的运动，引入人体手臂的臂型角定义。肩关节、肘关节和腕关节可以构成一个平面，定义这个平面为臂平面。颈部和肩关节形成的向量记为肩关节和腕关节形成的向量记为定义向量和形成的平面为参考平面。臂平面与参考平面生成的夹角，定义为臂型角，记作ψ。通过臂型角ψ可以描述肘关节绕方向的旋转运动。如图3，已知S点、E点和W点的坐标，即可求得E点到直线SW的垂足O点的坐标，这样向量就可以确定。已知B点和S点的坐标，可得向量将向量平移并通过O点，可得向量向量和向量的夹角即为手臂臂型角ψ，则已知B点和H点的坐标，可以求得手相对肩关节中点的位置，记为BPH，BPH＝[(xH-xB),(yH-yB),(zH-zB)]式中，xH，yH和zH为手的坐标，xB，yB和zB为肩关节中心的坐标。BPH描述了手臂末端相对于肩部中心的位置，缺少对手臂末端姿态的描述。根据手掌，手末端和拇指的位置关系，定义手掌的坐标系如图所示。其中，根据已知腕关节W和手掌位置H的坐标，可以求得向量该方向记为ZH。手掌点H，拇指点T和手掌末端HT可以构成一个平面，XH垂直于该平面，YH是根据ZH和XH由笛卡尔坐标系确定。手臂末端的姿态可以描述为BRH＝[XH,YH,ZH]。这样，基于臂型角的人体手臂骨骼，通过臂型角和手臂末端坐标系完整的描述出来了。运用手臂臂型角ψ和手掌位置BPH和姿态BRH，即可计算本发明所针对的仿人机械臂控制所需的七个自由度，将这七个自由度输入到仿人机械臂运动学模型中可以控制机械臂的运动。3、臂型角参数化的仿人机械臂仿人机械臂采用串联球形关节-旋转关节-球形关节的形式(简称S-R-S)，上述三个关节互相垂直相交于一点，形成球铰，机械臂的肩关节、肘关节、腕关节(如图5所示)，即采用这种S-R-S形式的球铰构型。这种构型能够最大程度的模拟人体的手臂，根据这种构型设计了人形机械臂。在本实施例中，为了增加机械臂的运动范围，肘关节设计有一定的位置偏置。机械臂在工作过程中的一种姿态可以用图6表示，其中等效的肩关节中点、肩关节、肘关节、腕关节和末端分别记为B'、S'、E'、W'和H'。1号关节位于B'和S'之间，1号连杆连接1号关节和S'，2号连杆连接S'和3号关节，3号连杆连接3号关节和E'，4号连杆连接E'和5号关节，5号连杆连接5号关节和W'，6号连杆连接W'和7号关节，7号连杆连接7号关节和H'；S'、E'、W'分别是2号、4号、6号关节。由此，已可以定义出能够准确、完整地描述该仿人机械臂运动形态的自由度角度θi(i＝1,2,…,7)，θi表示i号连杆绕i号关节轴线的转角。根据上述S-R-S构型的仿人机械臂，可以构建与人体手臂等效的机械臂运动学求解算法。定义机械臂的臂型角如下，机械臂等效的肩关节S'、肘关节E'和腕关节W'构成的平面称为臂平面，机械臂等效的肩关节中点B'、肩关节S'和腕关节W'构成的平面称为参考平面，臂平面与参考平面形成的角度称为机械臂的臂型角ψ′，ψ′＝ψ且ψ′取值范围与ψ一样为[-π,π]。在后文的计算中，将直接用ψ来替代ψ′进行计算。如图8已知仿人机械臂末端的位置和姿态(前面已由体感设备读取人体动作经映射得到)，可以得到肩关节S'到腕关节W'的向量初始状态用S0'、E0'和W0'表示，t时刻的状态用S'、E'和W'表示。则与的夹角即为肘部关节角θ4，由于θ4在机械臂的臂平面上，θ4不会引起ψ的变化，ψ也不会引起θ4的变化，所以θ4与ψ无关，根据余弦定理可以求得初始状态下和的夹角以及t时刻时和的夹角则肘关节的角度值为已知臂型角ψ，可以得到臂平面相对于参考平面的空间向量关系的旋转矩阵，记为0Rψ。同样，当3号坐标系的X3O3Y3平面与参考平面重合时，可以得到3号坐标系相对于基座标系的旋转矩阵，记为当3号坐标系的X3O3Y3平面与参考平面不重合时，根据所求的臂平面相对于参考平面的旋转矩阵和3号坐标系相对于基座标系的旋转矩阵，由此3号坐标系相对于基坐标系的姿态0R3可以表示为0Rψ与的点乘乘积。而根据机械臂正向运动学，同样可以获得3号坐标系相对于基坐标系的旋转矩阵0R3的以θ1、θ2和θ3来表达的另一种表达式。两种不同表达式的对应项相等，可以求得关节角θ1、θ2和θ3。同理，可以构建4号坐标系相对于7号坐标系的旋转矩阵7R4。而根据机械臂正向运动学，也可以获得4号坐标系相对于7号坐标系的旋转矩阵7R4的以θ5、θ6和θ7来表达的另一种表达式，7R4的两个不同的旋转矩阵表达式对应项相等，可以计算得到θ5、θ6和θ7的值。至此，已经获得仿人机械臂的全部七个运动学解。图9、图10分别是仿人机械臂体感控制系统示意图和仿人机械臂控制流程示意图。更具体的计算过程如下所示。1、建立机械臂的数学模型根据机械臂本体的结构构型，建立相对应的DH(Denavit-Hartenberg)坐标系。机械臂的数学模型及DH坐标系如图6所示。如图7，以i号关节+1的轴线为Zi轴，平行于i号关节的轴线的方向为Yi轴建立关于i号连杆的i号坐标系(i＝1～6)；以B'为原点，1号关节的轴线为Z0轴，平行于1号关节的轴线的方向为Y0轴建立坐标系0作为基坐标系(又称世界坐标系)；以H'为原点，关节7的轴线为Z0轴，平行于关节6的轴线的方向为Y7轴建立坐标系7作为机械臂末端坐标系。Xi是根据Zi和Yi由右手定则确定，i＝0,1,…,7。通过建立以上坐标系，定义如下参数：ai等于Zi-1和Zi之间沿Xi方向的距离，i＝1～7；di等于Xi-1和Xi之间沿Zi方向的距离，i＝1～7。DH坐标系建立以后，ai和di即为已知。在本实施例中，由于机械臂上连杆1、连杆2、关节2没有偏置，所以反应在DH坐标系中，Z1、Y2均与关节2的轴线重合，坐标系1与坐标系2的原点重合，即a1、a2＝0，d2＝0。同理，a5、a6、a7＝0，d2、d4、d6＝0。2、求解θ4在已知基坐标系中机械臂的末端位置0P7时(由人体的BPH直接映射得到)，S'点、W'点以及向量的值是唯一的。根据前面的定义，实际上1号关节轴线的方向与肩关节S'和腕关节W'方向形成的平面即为机械臂的参考平面。已知基坐标系中机械臂末端位姿0P7和0R7(分别由人体的BPH和BRH映射得到)，向量可以表示为：请参考图7、8，任意状态下，肩部、肘部和腕部的几何关系可以用图8表示。其中虚线部分为机械臂的初始状态，初始状态用S0'、E0'和W0'表示。实线部分为机械臂在t时刻的状态，用S'、E'和W'表示。图中与的夹角即为肘部关节角θ4。由于θ4在机械臂的臂平面上，θ4不会引起ψ的变化，ψ也不会引起θ4的变化，所以θ4与ψ无关：θ4=θ40-θ4t---(2)]]>cosθ40=S0′E0′2+E0′W0′2-S0′W0′22S0′E0′·E0′W0′]]>cosθ4t=S′E′2+E′W′2-S′W′22S′E′·E′W′]]>其中，表示初始状态下和的夹角；表示在t时刻时和的夹角。由图8中的几何关系可知，S0′E0′=S′E′=d32+a32,]]>S0W0＝d3+d5，E0′W0′=E′W′=d52+a42]]>在本实施例中，由图8可以看出，初始状态下，θ4＝0，机械臂伸直。3、求解θ1、θ2、θ3、θ5、θ6、θ7请参考图7，在基坐标系中，臂平面相对于参考平面的旋转矩阵为：式中，I3为3×3的单位阵，为向量的叉乘因子。当3号坐标系的X3O3Y3平面与参考平面重合时，ψ＝0，3号坐标系与基座标系之间的空间向量关系用来表示，0x3，0y3和0z3分别表示3号坐标系的三个方向向量：向量l是垂直于参考平面的向量：z03=-l|l|---(5)]]>由于肘关节偏置a3的存在，0y3相对于基坐标系的角度γ实际上由两部分组成，分别是转动形成的角度α和偏置产生的角度β：tanβ=a3d3---(7)]]>γ＝α+β(8)0y3相对于基坐标系的旋转矩阵为：R(l,γ)＝I3+[l×]sin(γ)+[l×]2(1-cos(γ))(9)式中，I3为3×3的单位阵，l×为向量l的叉乘因子0x3＝0y3×0z3(11)根据公式(5)(10)(11)可得：R3ψ=00=[x03,y03,z03]---(12)]]>则当3号坐标系与参考平面不重合时，3号坐标系相对于基坐标系的姿态可以表示为：R03=R0ψ·R3ψ=00---(13)]]>而根据正运动学方程可得0R3的另一种表达式如下：R03=s1s3+c1c2c3-c1s2s1c3-c1c2s3-c1s3+s1c2c3-s1s2c1c3-s1c2s3c3s2c2-s2s3---(14)]]>式中，si＝sinθi，ci＝cosθi，i＝1、2、3。由于方程(13)(14)右侧的对应项相等，即可据此求出θ1、θ2和θ3。同理，4号坐标系相对于7号坐标系的姿态有如下两种表达方式：R74=R7T0·R0ψ·R3ψ=00·R34---(15)]]>R74=c5c6c7-s5s7-c5c6s7-s5c7c5s6s5c6c7+c5s7-s5c6s7-c5c7s5s6-s6c7s6s7c6---(16)]]>式中，si＝sinθi，ci＝cosθi，i＝5、6、7；-机械臂的末端位置0P7的转置矩阵；3R4-4号坐标系相对于3号坐标系的旋转矩阵。由于方程(15)(16)右侧的对应项相等，即可据此求出θ5、θ6和θ7。按照以上计算过程，根据体感控制方法获取的人体手臂臂型角ψ和手掌位置BPH、姿态BRH，输入到仿人机械臂的运动学参数中按照相应公式进行计算，可以获得机械臂对应姿态下的运动学解，即获得此状态下的7个关节角度θ1～θ7，如图5。这种仿人机械臂体感控制方法和对应的仿人机械臂的算法结合，能够最大程度的模拟人体手臂的姿态从而实现运动控制。4具有虚拟场景显示的控制平台本实施例的机械臂体感控制平台，基于MFC开发。KinectV2SDK提供了基于VC++的开发接口，通过调用SDK的函数实现获取人体骨骼数据。遥操作开始和停止功能通过体感来控制，当系统处于待机状态时，通过体感捕捉人体手臂动作，当手臂动作符合预定动作后，遥操作端会发出系统开始体感控制指令。在人体手臂动作后，向仿人机械臂系统发送相应的动作指令。机械臂控制都是通过左臂来完成，当右臂向上举起时，系统判断为停止。机械臂体感控制平台的虚拟模型显示，通过OpenInvertor开发包来实现。OpenInvertor通过建立节点信息来更新虚拟场景，各节点的位置信息通过机械臂的连杆的相对位置矩阵来获得。机械臂体感控制平台的虚拟模型，还具有碰撞检测的功能。安全预警模型旨在预测机器人的运动情况，通过预测，可以预先知道机器人哪些动作是安全的，哪些动作是不安全的，从而指导用户快速实时地做出合理的决策，提高交互效率。安全预警算法是建立在虚拟现实技术上的一套安全保障算法，在位置信息可以预测的情况下，运用碰撞检测算法等手段进行安全预测。在虚拟场景中，计算机可以根据虚拟场景提供的机器人以及环境物体的位置信息，预测将要发生的碰撞，从而及早提醒操作者进行相应策略的调整。5基于EntherCAT总线通信方式所述机械臂体感控制平台通过EntherCAT总线方式与仿人机械臂的控制系统进行通信。使用倍福公司开发的TwinCAT软件作为开发包，在遥操作端进行总线通信。经过TwinCAT封装的EntherCAT通信方式，通过调用ADSnet来实现。经过ADS通信的方式，通信周期可以缩小到1ms，提高了体感控制系统的跟随速度。本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。当前第1页1 2 3