一种绳驱机械臂三维臂型测量方法及装置制造方法及图纸

一种绳驱机械臂三维臂型测量方法，包括如下步骤：S1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；S2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；S3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θ

A measuring method and device for three-dimensional arm shape of rope drive manipulator

【技术实现步骤摘要】
一种绳驱机械臂三维臂型测量方法及装置
本专利技术涉及绳驱机械臂，特别是一种绳驱机械臂三维臂型测量方法及装置。
技术介绍
绳驱机械臂为了保证整臂的轻量化和灵活性，往往采用绳驱这种驱动源后置的设计方案。这类设计使用多组绳索提供机械臂关节运动所需的驱动力和力矩，具有轻量、灵活、柔顺等优势，特别适合于狭小空间下的避障作业。绳驱机械臂可以穿越航天器的桁架结构和组件间隙，深入到结构内部进行探测、维修等任务。因此，绳驱机械臂的灵活性、柔顺性和细长特点使得其在空间狭小复杂环境中具有广阔的应用前景。绳驱机械臂模型如图1所示。该绳驱机械臂主要由若干个关节组成，关节之间采用十字轴进行连接，如图2所示，其中具有三根绳索L1-L3。绳驱机械臂的三维臂型检测问题始终是业界的难题。在常见的三维绳驱机械臂的臂型检测系统中，可以检测放置在绳驱机械臂上的靶球的坐标点，但无法检测出绳驱机械臂的臂型。因为绳驱机械臂本身具有一定的尺寸，靶球只能放在绳驱机械臂的外侧，不能直接安放在中轴线的位置，通过检测外侧靶球的坐标，不能直接还原出绳驱机械臂的关节角度。目前的三维相机可以精确地检测靶球坐标，但是没有相应的方法来准确得到绳驱机械臂的构形。
技术实现思路
本专利技术的主要目的在于克服现有技术的上述缺陷，提供一种能够准确测量绳驱机械臂三维臂型的测量方法及装置。为实现上述目的，本专利技术采用以下技术方案：一种绳驱机械臂三维臂型测量方法，包括如下步骤：S1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；S2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；S3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角S4、根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；S5、根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。进一步地：步骤S2包括：第i节关节上的两个靶球的坐标分别为和靶球在中轴线的投影距离为d，靶球坐标投影到中轴线的坐标分别对应为和靶球与中轴线构成的平面法向量为ni，第i节坐标系原点位于第i节与第i-1节的旋转轴中心Oi处，中轴线投影到xoy平面的投影直线与x轴的夹角为与之间的夹角为θi；通过三个约束关系来求解每一个靶球投影点的坐标，如下：Oi与和在同一个平面上，按照公式(1)计算得到该平面的法向量ni：由ni垂直于向量得到第一个约束关系：根据投影关系有垂直于得到第二个约束关系：根据投影距离d得到第三个约束关系：综合以上三个约束关系求解得到的坐标，同理，求得的坐标。步骤S3包括：将第i节关节的坐标结合第i-1节关节的和坐标，计算夹角θi：根据坐标的逐步递推，得到坐标系变换矩阵，用对应的关节坐标系表示每个关节上的点的坐标；坐标变换公式如下所示：其中，pi表示第i个坐标系的点，pi相对于第i个坐标系的原点Oi的位置关系与第一个关节上的点p1相对于O1的位置关系相同，表示从第i-1个坐标系到第i个坐标系的变换矩阵；根据公式(6)将前面计算得到的每个关节上的靶球投影坐标换算成当前坐标系下的坐标，再将该坐标投影到对应关节的xoy平面上，进而得到投影直线与x轴之间的夹角即关节的旋转角。步骤S4包括：计算x方向以及y方向的旋转矩阵以及平移矩阵T0：将旋转矩阵以及平移矩阵T0相乘得到关节的变换矩阵Ti：即，将式(7)～(9)代入式(10)得到关节的变换矩阵Ti：通过式(12)表示的坐标点的对应关系计算得到αi和βi：一种绳驱机械臂三维臂型测量装置，包括：多个靶球；用于固定靶球的工装，所述工装在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；多个相机，用于跟踪拍摄靶球，其经配置使被跟踪的靶球标记点至少同时有两台相机捕捉到；处理器，与所述相机连接，用于：根据相机拍摄的图像确定靶球的坐标；根据几何关系将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。进一步地，所述多个相机包括5个相机，其中4个相机通过转接件安装在三维吊丝微重力试验系统顶部的4个顶点，第5个相机安装在高度低于所述4个顶点的1根长横梁的中部。一种绳驱机械臂三维臂型测量方法，包括前述测量方法的步骤S2-S5。一种计算机可读存储介质，所述存储介质包括存储的计算机程序，其中，所述计算机程序运行时由处理器执行所述测量方法的步骤S2-S5。一种绳驱机械臂三维臂型测量装置，包括处理器和计算机可读存储介质，所述存储介质包括存储的计算机程序，其中，所述计算机程序运行时由所述处理器执行所述测量方法的步骤S2-S5。本专利技术具有如下有益效果：本专利技术针对绳驱机械臂的三维空间运动特点，通过将靶球固定在平行于绳驱机械臂关节中轴线的位置的靶球安装方式，使用本专利技术的运动学算法，可测得三维空间运动状态下的每个关节的角度，从而准确测量重建出绳驱机械臂的三维臂型。相对于传统直接利用外侧靶球的坐标进行测量的方法，本专利技术可以更加准确地测出绳驱机械臂的三维臂型。附图说明图1为绳驱机械臂模型图。图2为单关节机械结构图。图3为本专利技术实施例中的三维运动捕捉系统示例图。图4为本专利技术实施例中的臂型检测系统的相关部件示意图。图5为本专利技术实施例中的单关节的几何简化图。图6为本专利技术实施例中的靶球坐标投影到中轴线的原理图。图7为本专利技术实施例中的靶球坐标换算关节转角原理图。具体实施方式以下对本专利技术的实施方式作详细说明。应该强调的是，下述说明仅仅是示例性的，而不是为了限制本专利技术的范围及其应用。需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是耦合或连通作用。需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本专利技术实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本专利技术本文档来自技高网...

【技术保护点】
1.一种绳驱机械臂三维臂型测量方法，其特征在于，包括如下步骤：/nS1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；/nS2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；/nS3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θ

【技术特征摘要】
1.一种绳驱机械臂三维臂型测量方法，其特征在于，包括如下步骤：
S1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；
S2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；
S3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角
S4、根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；
S5、根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。


2.如权利要求1所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤S2包括：
第i节关节上的两个靶球的坐标分别为和靶球在中轴线的投影距离为d，靶球坐标投影到中轴线的坐标分别对应为和靶球与中轴线构成的平面法向量为ni，第i节坐标系原点位于第i节与第i-1节的旋转轴中心Oi处，中轴线投影到xoy平面的投影直线与x轴的夹角为与之间的夹角为θi；
通过三个约束关系来求解每一个靶球投影点的坐标，如下：
Oi与和在同一个平面上，按照公式(1)计算得到该平面的法向量ni：
ni＝OiPi1×Pi1Pi2(1)
由ni垂直于向量得到第一个约束关系：



根据投影关系有垂直于得到第二个约束关系：



根据投影距离d得到第三个约束关系：



综合以上三个约束关系求解得到的坐标，同理，求得的坐标。


3.如权利要求1或2所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤S3包括：
将第i节关节的坐标结合第i-1节关节的和坐标，计算夹角θi：



根据坐标的逐步递推，得到坐标系变换矩阵，用对应的关节坐标系表示每个关节上的点的坐标；坐标变换公式如下所示：



其中，pi表示第i个坐标系的点，pi相对于第i个坐标系的原点Oi的位置关系与第一个关节上的点p1相对于O1的位置关系相同，表示从第i-1个坐标系到第i个坐标系的变换矩阵；
根据公式(6)将前面计算得到的每个关节上的靶球投影坐标换算成当前坐标系下的坐标，再将该坐标投影到对应关节的xoy平面...

【专利技术属性】
技术研发人员：孟得山，王学谦，梁斌，黄少平，米奕霖，徐文福，
申请(专利权)人：清华大学深圳国际研究生院，
类型：发明
国别省市：广东;44