[发明专利]一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法

摘要

本发明公开了一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，采用位置跟踪器精确确定手臂末端位置，根据运动约束得到有限组冗余机械臂逆解，再通过手臂肘部的位置跟踪器来确定唯一逆解，使得冗余机械臂与人体手臂形态相似，达到控制机械臂的效果。本发明与现有技术相比，更方便，更直观解决控制冗余机械臂的问题。

主权项

基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，其特征在于，包括以下步骤：1)建立虚拟手臂模型、建立虚拟机械臂模型；在3DMAX中建立静态模型，首先对虚拟手臂和虚拟机械臂进行整体建模，然后再将整体模型各部分进行分离，其中虚拟手臂分离为虚拟上臂、虚拟前臂、虚拟手掌和虚拟手指，每个手指按真实人手关节分为多个指段，在临近的两个分离目标之间加入关节球；机械臂分成七个部分，每个部分为机械臂的连杆；在静态模型的各个关节处添加虚拟对象，并保证各个虚拟对象坐标轴指向相同；在图解视图中建立树状层次结构，通过分析人类手臂运动规律来建立树状层次结构的动态模型；2)数据手套、位置跟踪器采集手臂数据并处理；用户穿戴数据手套，将三个位置跟踪器分别放置在手背和前臂上，将手背和前臂近似看作刚体，1号位置跟踪器放在手背靠近手腕处，2号位置跟踪器放在前臂靠近肘处，3号位置跟踪器放在肩部；将数据手套和位置跟踪器的USB接口串口接入计算机，启动所有设备，用户需要进行一系列动作，包括：将手掌握拳后展开以及将手臂向前平行伸直，来标定数据手套和位置跟踪器的信息；虚拟手指则通过读取数据手套手指根部和手指近端的传感器来读入信息，根据人体构造，手指远端DIP随着手指近端PIP运动，运动关系为θDIP＝θPIP2/3，其中，θDIP为手指远端关节转角，θPIP为手指近端关节转角；3)由机械臂期望末端位置逆解出冗余机械臂多组构型；采用关节角参数化方法逆解冗余机械臂，θ1、θ2、θ3、θ4、θ5、θ6和θ7分别为机械臂的七个关节的旋转角度，由期望的机械臂末端位置，根据机械臂的臂型特征，推导以θ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)+s7c2s345nxs1-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+s6c2c345)+d6c2s345+a3c2c3+a4s2c34pxs1-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程序中，再操纵虚拟手臂各个关节处的虚拟对象，实现通过手臂对虚拟机械臂的控制。