CN105904457B - 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 - Google Patents
一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 Download PDFInfo
- Publication number
- CN105904457B CN105904457B CN201610322280.7A CN201610322280A CN105904457B CN 105904457 B CN105904457 B CN 105904457B CN 201610322280 A CN201610322280 A CN 201610322280A CN 105904457 B CN105904457 B CN 105904457B
- Authority
- CN
- China
- Prior art keywords
- msub
- mtd
- mrow
- mtr
- arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J21/00—Chambers provided with manipulation devices
- B25J21/02—Glove-boxes, i.e. chambers in which manipulations are performed by the human hands in gloves built into the chamber walls; Gloves therefor
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40367—Redundant manipulator
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，采用位置跟踪器精确确定手臂末端位置，根据运动约束得到有限组冗余机械臂逆解，再通过手臂肘部的位置跟踪器来确定唯一逆解，使得冗余机械臂与人体手臂形态相似，达到控制机械臂的效果。本发明与现有技术相比，更方便，更直观解决控制冗余机械臂的问题。
Description
【技术领域】
本发明属于人机交互领域，具体涉及一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法。
【背景技术】
人机交互强调“以人为本”“自然、和谐”的交互方式，以实现人机高效的合作，通过可穿戴设备的交互使得人脑得到“直接”和有效的扩充与延伸。
7自由度冗余机械臂相较于6自由度而言，具有避开奇异位置，障碍回避，改善动态性能等特点。但是多数冗余机械臂的构型与人手臂的构型不同，这就导致手臂与机械臂的交互存在不准确性。针对人体手臂与七自由度机械臂构型之间的交互困难问题，本发明采用数据手套和位置跟踪器采集使用者手臂的位置姿态信息，通过一种空间映射方法，得到机械臂的臂型，实现人体手臂对异构型七自由度机械臂的控制。
【发明内容】
本发明的目的在于弥补现有手臂控制异构型7自由度冗余机械臂方法的不足，提供一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法。
为达到上述目的，本发明采用以下技术方案予以实现：
基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，包括以下步骤：
1)建立虚拟手臂模型、建立虚拟机械臂模型；
在3DMAX中建立静态模型，首先对虚拟手臂和虚拟机械臂进行整体建模，然后再将整体模型各部分进行分离，其中虚拟手臂分离为虚拟上臂、虚拟前臂、虚拟手掌和虚拟手指，每个手指按真实人手关节分为多个指段，在临近的两个分离目标之间加入关节球；机械臂分成七个部分，每个部分为机械臂的连杆；
在静态模型的各个关节处添加虚拟对象，并保证各个虚拟对象坐标轴指向相同；在图解视图中建立树状层次结构，通过分析人类手臂运动规律来建立树状层次结构的动态模型；
2)数据手套、位置跟踪器采集手臂数据并处理；
用户穿戴数据手套，将三个位置跟踪器分别放置在手背和前臂上，将手背和前臂近似看作刚体，1号位置跟踪器放在手背靠近手腕处，2号位置跟踪器前臂靠近肘处，3号位置跟踪器放在肩部；
将数据手套和位置跟踪器的USB接口串口接入计算机，启动所有设备，用户需要进行一系列动作，包括：将手掌握拳后展开以及将手臂向前平行伸直，来标定数据手套和位置跟踪器的信息；
虚拟手则通过读取数据手套手指根部和手指近端的传感器来读入信息，根据人体构造，手指远端DIP随着手指近端PIP运动，运动关系为θDIP＝θPIP2/3，其中，θDIP为手指远端关节转角，θPIP为手指近端关节转角；
3)由机械臂期望末端位置逆解出冗余机械臂多组构型；
采用关节角参数化方法逆解冗余机械臂，由期望的机械臂末端位置，根据机械臂的臂型特征，推导以θ1、θ2中的一个作为给定参数时其他关节角的表达式，得到8组解，分别为8种机械臂的构型；
关节角参数化方法如下：
假设机械臂的末端位姿为：
其中[nx,ny,nz]T、[ax,ay,az]T、[sx,sy,sz]T分别为末端坐标系在初始0号坐标系下的单位矢量，[px,py,pz]T为末端坐标系原点的位置；
相邻连杆坐标系间的位置和姿态用其次变换矩阵i-1Ti表示：
其中c＝cos，s＝sin，且有
0T7＝0T1 1T2…6T7 (1.3)
结合上面等式得到如下关系：
将(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
假设机械臂的末端位姿为：
其中[nx,ny,nz]T、[ax,ay,az]T、[sx,sy,sz]T分别为末端坐标系在初始0号坐标系下的单位矢量，[px,py,pz]T为末端坐标系原点的位置；
相邻连杆坐标系间的位置和姿态用其次变换矩阵i-1Ti表示：
其中c＝cos，s＝sin，且有
0T7＝0T1 1T2…6T7 (1.3)
结合上面等式和表1得到如下关系：
将(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程序中，再操纵虚拟手臂各个关节处的虚拟对象，达到手臂控制虚拟机械臂的目的。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。
Claims (1)
1.基于位置跟踪器及数据手套的异构型冗余机械臂控制方法，其特征在于，包括以下步骤：
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种机械臂的构型；
关节角参数化方法如下：
假设机械臂的末端位姿为：
<mrow>
<msub>
<mmultiscripts>
<mi>T</mi>
<mn>0</mn>
</mmultiscripts>
<mn>7</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1.1</mn>
<mo>)</mo>
</mrow>
</mrow>
其中[nx,ny,nz]T、[ax,ay,az]T、[sx,sy,sz]T分别为末端坐标系在初始0号坐标系下的单位矢量，[px,py,pz]T为末端坐标系原点的位置；
相邻连杆坐标系间的位置和姿态用其次变换矩阵i-1Ti表示：
<mrow>
<msub>
<mmultiscripts>
<mi>T</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1.2</mn>
<mo>)</mo>
</mrow>
</mrow>
其中c＝cos，s＝sin，且有
0T7＝0T1 1T2…6T7 (1.3)
结合上面等式得到如下关系：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>n</mi>
<mi>x</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>n</mi>
<mi>y</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>c</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>+</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>7</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>n</mi>
<mi>z</mi>
</msub>
<mo>=</mo>
<msub>
<mi>c</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>7</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>n</mi>
<mi>x</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>n</mi>
<mi>y</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>c</mi>
<mn>7</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>7</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s</mi>
<mi>x</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mi>y</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s</mi>
<mi>z</mi>
</msub>
<mo>=</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s</mi>
<mi>x</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mi>y</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>s</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>+</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>7</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<mo>=</mo>
<msub>
<mi>s</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>7</mn>
</msub>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>d</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>d</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>4</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>34</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>p</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>d</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>d</mi>
<mn>7</mn>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>d</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>4</mn>
</msub>
<msub>
<mi>s</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>34</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msub>
<mi>d</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<msub>
<mi>s</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>d</mi>
<mn>6</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>4</mn>
</msub>
<msub>
<mi>c</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>d</mi>
<mn>7</mn>
</msub>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<msub>
<mi>s</mi>
<mn>345</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>4</mn>
</msub>
<msub>
<mi>s</mi>
<mn>34</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1.4</mn>
<mo>)</mo>
</mrow>
</mrow>
将(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程序中，再操纵虚拟手臂各个关节处的虚拟对象，实现通过手臂对虚拟机械臂的控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610322280.7A CN105904457B (zh) | 2016-05-16 | 2016-05-16 | 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610322280.7A CN105904457B (zh) | 2016-05-16 | 2016-05-16 | 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105904457A CN105904457A (zh) | 2016-08-31 |
| CN105904457B true CN105904457B (zh) | 2018-03-06 |
Family
ID=56749219
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610322280.7A Active CN105904457B (zh) | 2016-05-16 | 2016-05-16 | 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105904457B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106378772B (zh) * | 2016-11-28 | 2019-02-26 | 国网山东省电力公司济南市长清区供电公司 | 一种基于杠杆原理搬运用穿戴式机械辅助手臂 |
| CN106584461B (zh) * | 2016-12-21 | 2019-02-15 | 西安科技大学 | 多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法 |
| US10379613B2 (en) * | 2017-05-16 | 2019-08-13 | Finch Technologies Ltd. | Tracking arm movements to generate inputs for computer systems |
| CN108762495B (zh) * | 2018-05-18 | 2021-06-29 | 深圳大学 | 基于手臂动作捕捉的虚拟现实驱动方法及虚拟现实系统 |
| CN111844005B (zh) * | 2020-07-08 | 2022-06-28 | 哈尔滨工业大学 | 一种应用于隧道湿喷的2r-p-2r-p-2r机械臂运动规划方法 |
| CN111872938B (zh) * | 2020-07-30 | 2022-01-25 | 清华大学 | 空间三维大尺度运动学仿真系统及方法 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH04369004A (ja) * | 1991-06-17 | 1992-12-21 | Yaskawa Electric Corp | マニピュレータのインピーダンス制御方法 |
| KR101086361B1 (ko) * | 2009-03-03 | 2011-11-23 | 삼성중공업 주식회사 | 로봇의 자세 제어 방법 및 그 장치 |
| CN102814814B (zh) * | 2012-07-31 | 2015-07-01 | 华南理工大学 | 一种双臂机器人基于Kinect的人机交互方法 |
| CN103386683B (zh) * | 2013-07-31 | 2015-04-08 | 哈尔滨工程大学 | 一种基于Kinect的体感控制机械臂方法 |
| CN104635762B (zh) * | 2015-01-13 | 2017-06-06 | 北京航空航天大学 | 一种面向srs仿人手臂的自运动角计算方法 |
| CN104808510B (zh) * | 2015-02-16 | 2017-07-21 | 中国航天员科研训练中心 | 虚拟航天员多层次运动控制仿真方法 |
| CN105014677B (zh) * | 2015-07-07 | 2016-07-20 | 西安交通大学 | 基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法 |
| CN105260029B (zh) * | 2015-11-24 | 2018-04-24 | 哈尔滨工业大学 | 基于人手运动功能复现的仿人手结构实现方法 |
-
2016
- 2016-05-16 CN CN201610322280.7A patent/CN105904457B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN105904457A (zh) | 2016-08-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105904457B (zh) | 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 | |
| CN107943283B (zh) | 基于手势识别的机械臂位姿控制系统 | |
| CN107953331B (zh) | 一种应用于人形机器人动作模仿的人体姿态映射方法 | |
| CN109116992B (zh) | 一种用于虚拟手力反馈交互的碰撞响应系统 | |
| CN104809276B (zh) | 一种多手指机器人动力学解析模型及其建模方法 | |
| CN110815189A (zh) | 基于混合现实的机器人快速示教系统及方法 | |
| CN106041912A (zh) | 主从式蛇形机器人系统及其位置控制方法 | |
| CN105094373A (zh) | 操作工业机器人的手势采集装置和相应手势采集方法 | |
| Hu et al. | Calibrating human hand for teleoperating the HIT/DLR hand | |
| CN109048897A (zh) | 一种主从机器人遥操作的方法 | |
| CN102198660A (zh) | 基于脑-机接口的机械手臂控制系统及动作命令控制方案 | |
| CN111300408A (zh) | 一种结合形似与神似的仿人双臂机器人运动规划控制方法 | |
| CN108334198A (zh) | 基于增强现实的虚拟雕塑方法 | |
| CN113070877B (zh) | 一种面向七轴机械臂视觉示教的可变姿态映射方法 | |
| CN203680324U (zh) | 一种外骨架穿戴式数据手套 | |
| CN111185906B (zh) | 一种基于Leap Motion的灵巧手主从控制方法 | |
| CN112650079B (zh) | 一种七自由度机器人的运动学反解方法 | |
| CN108127667B (zh) | 一种基于关节角增量的机械臂体感交互控制方法 | |
| Abdel-Malek et al. | A mathematical method for ergonomic-based design: placement | |
| CN107422855B (zh) | 一种可提供夹持力感觉的波纹管式球囊型触觉手套及制作方法 | |
| CN115781666A (zh) | 一种用于机器人全身模仿系统的控制方法 | |
| Kong et al. | A novel approach to deriving the unit-homogeneous Jacobian matrices of mechanisms | |
| Bing et al. | System design and experiment of bionics robotic arm with humanoid characteristics | |
| Tian et al. | Nature grasping by a cable-driven under-actuated anthropomorphic robotic hand | |
| Mei et al. | Simulation Research on Motion Trajectory of PUMA 560 Manipulator Based on MATLAB |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |