【技术领域】本发明属于人机交互领域，具体涉及一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法。
背景技术：
人机交互强调“以人为本”“自然、和谐”的交互方式，以实现人机高效的合作，通过可穿戴设备的交互使得人脑得到“直接”和有效的扩充与延伸。7自由度冗余机械臂相较于6自由度而言，具有避开奇异位置，障碍回避，改善动态性能等特点。但是多数冗余机械臂的构型与人手臂的构型不同，这就导致手臂与机械臂的交互存在不准确性。针对人体手臂与七自由度机械臂构型之间的交互困难问题，本发明采用数据手套和位置跟踪器采集使用者手臂的位置姿态信息，通过一种空间映射方法，得到机械臂的臂型，实现人体手臂对异构型七自由度机械臂的控制。
技术实现要素：
本发明的目的在于弥补现有手臂控制异构型7自由度冗余机械臂方法的不足，提供一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法。为达到上述目的，本发明采用以下技术方案予以实现：基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，包括以下步骤：1)建立虚拟手臂模型、建立虚拟机械臂模型；在3DMAX中建立静态模型，首先对虚拟手臂和虚拟机械臂进行整体建模，然后再将整体模型各部分进行分离，其中虚拟手臂分离为虚拟上臂、虚拟前臂、虚拟手掌和虚拟手指，每个手指按真实人手关节分为多个指段，在临近的两个分离目标之间加入关节球；机械臂分成七个部分，每个部分为机械臂的连杆；在静态模型的各个关节处添加虚拟对象，并保证各个虚拟对象坐标轴指向相同；在图解视图中建立树状层次结构，通过分析人类手臂运动规律来建立树状层次结构的动态模型；2)数据手套、位置跟踪器采集手臂数据并处理；用户穿戴数据手套，将三个位置跟踪器分别放置在手背和前臂上，将手背和前臂近似看作刚体，1号位置跟踪器放在手背靠近手腕处，2号位置跟踪器前臂靠近肘处，3号位置跟踪器放在肩部；将数据手套和位置跟踪器的USB接口串口接入计算机，启动所有设备，用户需要进行一系列动作，包括：将手掌握拳后展开以及将手臂向前平行伸直，来标定数据手套和位置跟踪器的信息；虚拟手则通过读取数据手套手指根部和手指近端的传感器来读入信息，根据人体构造，手指远端DIP随着手指近端PIP运动，运动关系为θDIP＝θPIP2/3，其中，θDIP为手指远端关节转角，θPIP为手指近端关节转角；3)由机械臂期望末端位置逆解出冗余机械臂多组构型；采用关节角参数化方法逆解冗余机械臂，由期望的机械臂末端位置，根据机械臂的臂型特征，推导以θ1、θ2中的一个作为给定参数时其他关节角的表达式，得到8组解，分别为8种机械臂的构型；关节角参数化方法如下：假设机械臂的末端位姿为：T07=nxsxaxpxnysyaypynzszazpz0001---(1.1)]]>其中[nx,ny,nz]T、[ax,ay,az]T、[sx,sy,sz]T分别为末端坐标系在初始0号坐标系下的单位矢量，[px,py,pz]T为末端坐标系原点的位置；相邻连杆坐标系间的位置和姿态用其次变换矩阵i-1Ti表示：Ti-1i=cθi-sθicαisθisαiαicθisθicθicαi-cθisαiαisθi0sαicαidi0001---(1.2)]]>其中c＝cos，s＝sin，且有0T7＝0T11T2…6T7(1.3)结合上面等式得到如下关系：{nxc1+nys1=c7(s2s6+c2c6c345)+s7c2s345nz=c7(-c2s6+s2c6c345)+s7s2s345nxs1-nyc1=c7c6s345-s7c345sxc1+sys1=c2s6c345-s2c6sz=s2s6c345+c2c6sxs1-syc1=s6s345axc1+ays1=s7(s2s6+c2c6c345)-c7c2s345az=s7(-c2s6+s2c6c345)-c7s2s345pxc1+pys1=d3s2-d7(c6s2-s6c2c345)+d6c2s345+a3c2c3+a4c2c34pz-d1=-d3c2+d7(c6c2+s6s2c345)+d6s2s345+a3s2c3+a4s4c34pxs1-pyc1=d2+a3s3-d6c345+a4c345+d7s6s345+a4s34---(1.4)]]>将(1.4)整理化简得到θ1和θ2的关系式：(pz-d1-d7sz)c2+((d7sx-px)c1+(d7sy-py)s1)s2+d3＝0(1.5)式(1.5)表明，θ1和θ2任意给定其中一个都能够求出另一个，有两组可能解；式(1.4)整理简化得到θ6和θ7的关系式：c6＝szc2-(sxc1+sys1)s2(1.6)由(1.6)知，对于一组θ1和θ2得到θ6的两个值，进而求得与θ1、θ2、θ6相对应的θ7的值；根据式(1.1)、式(1.3)和式(1.4)可得到与θ1、θ2、θ6、θ7相对应的θ4、θ3、θ5；当给定θ1或θ2时，能够获得8组逆解；4)通过采集的手臂数据确定唯一机械臂构型；首先建立人体手臂坐标系Oxyz0，以人体肩部到手部位置跟踪器方向为x轴方向，使手臂与水平面平行且与肩部垂直，过肩关节做垂直于地面的垂线为z轴，正方向为地面指向肩关节，y轴为x轴和z轴的公垂线，正方向为人体向外，建立的坐标系随x轴变化；同理建造冗余机械臂坐标系Oxyz1，以第一关节的起始端到末端关节的末端的连线为x轴，正方向为第一关节指向末端关节末端的方向，冗余机械臂伸直时与x轴垂直且在以若干中间关节轴所在平面的为z轴，正方向向上，y轴为x轴和z轴的公垂线，坐标系随x轴变化；然后由上述方法建立得到的坐标系划分的八个象限作为八个区域，Oxyz0的区域为Ω01、Ω02、Ω03、Ω04、Ω05、Ω06、Ω07、Ω08八个区域，Oxyz1的区域为Ω11、Ω12、Ω13、Ω14、Ω15、Ω16、Ω17、Ω18八个区域，通过步骤2)得到的2号位置跟踪器所得的肘部位置与Ω0的八个区域进行比较，确定出肘部区域；最后分析步骤3)得到的八组冗余机械臂的臂型，选取第五个关节所在Ω1的区域与人臂肘部所在区域相同的臂型，进而确定准确臂型；5)对虚拟机械臂进行控制；操作者手臂的运动通过数据手套和两个位置跟踪器的测量，经由步骤3)得到处理后的采集数据，得到确定的手臂的位置姿态，通过步骤4)确定冗余机械臂的唯一准确臂型，将数据输入到MFC编译的OSG程序中，再操纵虚拟手臂各个关节处的虚拟对象，实现通过手臂对虚拟机械臂的控制。与现有技术相比，本发明具有以下有益效果：本发明采用位置跟踪器精确确定手臂末端位置，根据运动约束得到有限组冗余机械臂逆解，再通过手臂肘部的位置跟踪器来确定唯一逆解，使得冗余机械臂与人体手臂形态相似，达到控制机械臂的效果。本发明与现有技术相比，更方便，更直观解决控制冗余机械臂的问题。【附图说明】图1冗余机械臂的控制流程图。【具体实施方式】下面结合附图对本发明做进一步详细描述：参见图1，本发明基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，包括以下步骤：第一步：建立虚拟手臂模型、建立虚拟机械臂模型建立虚拟手臂和虚拟冗余机械臂是为了使人体手臂运动更直观的展现在操作者眼中，虚拟模型分为静态建模和动态建模两部分，在建模过程中如何做到数据量小、模型质量高是虚拟现实研究的难题。在3DMAX中建立静态模型，首先对虚拟手臂和虚拟机械臂进行整体建模，然后再将整体模型各部分进行分离，其中虚拟手臂分离为虚拟上臂、虚拟前臂、虚拟手掌和虚拟手指，每个手指按真实人手关节分为多个指段，在临近的两个分离目标之间加入关节球，以此解决虚拟手臂弯曲连接处裂缝的问题。机械臂分成七个部分，每个部分为机械臂的连杆。为了控制虚拟手臂模型，在静态模型的各个关节处添加虚拟对象，并保证各个虚拟对象坐标轴指向相同以方便模型操纵。在图解视图中建立树状层次结构，通过分析人类手臂运动规律来建立树状层次结构的动态模型。第二步：数据手套、位置跟踪器采集手臂数据用户穿戴数据手套，将三个位置跟踪器分别放置在手背和前臂上，为了方便数据采集和虚拟对象的控制，手背和前臂近似看作刚体，1号位置跟踪器放在手背靠近手腕处，2号位置跟踪器前臂靠近肘处，3号位置跟踪器放在肩部。将数据手套和位置跟踪器的USB接口串口接入计算机，启动所有设备，用户需要进行一系列动作，包括，将手掌握拳后展开，将手臂向前平行伸直，来标定数据手套和位置跟踪器的信息。虚拟手则通过读取数据手套手指根部和手指近端的传感器来读入信息，根据人体构造，手指远端(DIP)随着手指近端(PIP)运动，运动关系为θDIP＝θPIP2/3。第三步：由机械臂期望末端位置逆解出冗余机械臂多组构型采用关节角参数化方法逆解冗余机械臂，由期望的机械臂末端位置，根据机械臂的臂型特征，推导以θ1、θ2中的一个作为给定参数时其他关节角的表达式，得到8组解，分别为8种机械臂的构型。关节角参数化方法如下：设虚拟机械臂的D-H参数如表1假设机械臂的末端位姿为：T07=nxsxaxpxnysyaypynzszazpz0001---(1.1)]]>其中[nx,ny,nz]T、[ax,ay,az]T、[sx,sy,sz]T分别为末端坐标系在初始0号坐标系下的单位矢量，[px,py,pz]T为末端坐标系原点的位置；相邻连杆坐标系间的位置和姿态用其次变换矩阵i-1Ti表示：Ti-1i=cθi-sθicαisθisαiαicθisθicθicαi-cθisαiαisθi0sαicαidi0001---(1.2)]]>其中c＝cos，s＝sin，且有0T7＝0T11T2…6T7(1.3)结合上面等式和表1得到如下关系：nxc1+nys1=c7(s2s6+c2c6c345)+s7c2s345nz=c7(-c2s6+s2c6c345)+s7s2s345nxs1-nyc1=c7c6s345-s7c345sxc1+sys1=c2s6c345-s2c6sz=s2s6c345+c2c6sxs1-syc1=s6s345axc1+ays1=s7(s2s6+c2c6c345)-c7c2s345az=s7(-c2s6+s2c6c345)-c7s2s345pxc1+pys1=d3s2-d7(c6s2-s6c2c345)+d6c2s345+a3c2c3+a4c2c34pz-d1=-d3c2+d7(c6c2+s6s2c345)+d6s2s345+a3s2c3+a4s4c34pxs1-pyc1=d2+a3s3-d6c345+a4c345+d7s6s345+a4s34---(1.4)]]>将(1.4)整理化简得到θ1和θ2的关系式：(pz-d1-d7sz)c2+((d7sx-px)c1+(d7sy-py)s1)s2+d3＝0(1.5)式(1.5)表明，θ1和θ2任意给定其中一个都能够求出另一个，有两组可能解；式(1.4)整理简化得到θ6和θ7的关系式：c6＝szc2-(sxc1+sys1)s2(1.6)由(1.6)知，对于一组θ1和θ2得到θ6的两个值，进而求得与θ1、θ2、θ6相对应的θ7的值。根据式(1.1)、式(1.3)和式(1.4)可得到与θ1、θ2、θ6、θ7相对应的θ4、θ3、θ5。综上所述，当给定θ1或θ2时，可以获得8组逆解。第四步：通过采集的手臂数据确定唯一的机械臂构型首先建立人体手臂坐标系Oxyz0，以人体肩部到手部位置跟踪器方向为x轴方向，使手臂与水平面平行且与肩部垂直，过肩关节做垂直于地面的垂线为z轴，正方向为地面指向肩关节，y轴为x轴和z轴的公垂线，正方向为人体向外，建立的坐标系随x轴变化。同理建造冗余机械臂坐标系Oxyz1，以连杆1起始端到连杆7末端的连线为x轴，正方向为杆1指向杆7末端的方向，冗余机械臂伸直时与x轴垂直且在以杆3、4、5轴所在平面的为z轴，正方向向上，y轴为x轴和z轴的公垂线，坐标系随x轴变化。然后由上述方法建立得到的坐标系划分的八个象限作为八个区域，Oxyz0的区域为Ω01、Ω02、Ω03、Ω04、Ω05、Ω06、Ω07、Ω08八个区域，Oxyz1的区域为Ω11、Ω12、Ω13、Ω14、Ω15、Ω16、Ω17、Ω18八个区域，通过第二步得到的2号位置跟踪器所得的肘部位置与Ω0的八个区域进行比较，确定出肘部位于哪个区域。最后分析第三步得到的八组冗余机械臂的臂型，选取第五个关节所在Ω1的区域与人臂肘部所在区域相同的臂型，进而确定准确臂型。第五步：对机械臂进行控制操作者手臂的运动通过数据手套和两个位置跟踪器的测量，经由第三步得到处理后的采集数据，可以确定的将手臂位置姿态，通过第四步确定冗余机械臂的唯一准确臂型，将数据输入到MFC编译的OSG程序中，再操纵虚拟手臂各个关节处的虚拟对象，达到手臂控制虚拟机械臂的目的。以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。