一种绳驱机械臂三维臂型测量方法及装置与流程

本发明涉及绳驱机械臂，特别是一种绳驱机械臂三维臂型测量方法及装置。

背景技术：

绳驱机械臂为了保证整臂的轻量化和灵活性，往往采用绳驱这种驱动源后置的设计方案。这类设计使用多组绳索提供机械臂关节运动所需的驱动力和力矩，具有轻量、灵活、柔顺等优势，特别适合于狭小空间下的避障作业。绳驱机械臂可以穿越航天器的桁架结构和组件间隙，深入到结构内部进行探测、维修等任务。因此，绳驱机械臂的灵活性、柔顺性和细长特点使得其在空间狭小复杂环境中具有广阔的应用前景。

绳驱机械臂模型如图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，本发明实施例提供一种绳驱机械臂三维臂型测量方法，包括如下步骤：

s1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；

s2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；

s3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角

s4、根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；

s5、根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。

本发明实施例还提供一种绳驱机械臂三维臂型测量装置，包括：多个靶球；用于固定靶球的工装，所述工装在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；多个相机，用于跟踪拍摄靶球，其经配置使被跟踪的靶球标记点至少同时有两台相机捕捉到；处理器，与所述相机连接，用于：根据相机拍摄的图像确定靶球的坐标；根据几何关系将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。

在优选的实施例中，所述多个相机包括5个相机，其中4个相机通过转接件安装在三维吊丝微重力试验系统顶部的4个顶点，第5个相机安装在高度低于所述4个顶点的1根长横梁的中部。

本发明实施例还提供一种绳驱机械臂三维臂型测量方法，包括前述测量方法的步骤s2-s5。

相对于传统直接读取坐标的方法，本发明通过采集到的靶球坐标，可以重建出绳驱机械臂的三维构型，也就是其每一个关节的关节角度。该关节角度对于绳驱机械臂的实时控制有着重大意义，可用于运动补偿、规划控制等方面。本发明可用于测量各种不同机械机构绳驱机械臂的三维臂型。且从本发明测量得到的理论值与实际值对比来看，最大误差不超过0.3°，表明本发明可通过检测多关节连续型机械臂关节角度，进而准确重建其三维臂型。

以下结合附图进一步描述本发明具体实施例。

测量系统介绍

optitrack三维运动捕捉系统的5个optitrack相机通过转接件安装在三维吊丝微重力试验系统顶部的4个顶点和1根长横梁中部。安装完成后，开启三维运动捕捉系统专用motive软件，调整相机的焦距等使相机成像清晰，并确保被跟踪的靶球标记点在捕捉系统中至少同时有两台相机捕捉到。图3所示为optitrack三维运动捕捉系统图，其中示出了三维运动捕捉分系统的相机的部分安装示例。图4为臂型检测系统的相关部件示意图，其中示出了三维运动捕捉分系统的靶球安装。图中，①表示待多关节连续型机械臂；②表示固定靶球的工装，该工装可以保证每一个关节上的两个靶球与所在关节的中轴线平行；③表示靶球；④表示外部相机，其功能为检测靶球的世界坐标。

三维臂型计算方法

根据相机系统采集到的靶球坐标，利用机械臂运动学迭代算法对臂型重建。具体步骤如下：首先根据几何关系，将靶球坐标投影到臂杆中轴线上。其次，计算每一小节之间的异面直线夹角θi以及旋转角进而计算机械臂对应的关节十字轴方向角αi和βi。最后根据每个关节的关节角，从根部往末端迭代得到该机械臂的臂型。下面将描述每一步骤的具体细节。

图5是单关节的几何简化图，两个靶球用t表示，过绳圆盘用b表示，中轴线用a表示。图6为靶球坐标投影到中轴线的原理图，其中定义第i节的坐标定义关系：两个靶球的坐标分别为和靶球t在中轴线a的投影距离为d，靶球坐标投影到中轴线的坐标分别对应为和靶球与中轴线构成的平面法向量为ni，第i节坐标系原点位于第i节与第i-1节的旋转轴中心oi处。图7为靶球坐标换算关节转角原理图，其中定义中轴线投影到xoy平面的投影直线与x轴的夹角为与之间的夹角为θi。

首先将靶球坐标投影到中轴线上。因为每一个靶球投影点的坐标有三个未知量，因此需要三种约束关系来求解。

如图6所示，因为中轴线经过oi，因此oi与和在同一个平面上，进而可以得到该平面的法向量ni的计算公式为：

由ni垂直于向量可以得到第一个约束关系：

再根据投影关系有垂直于因此可以得到第二个约束关系：

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

综合以上三个方程，可以求解得到的坐标，同理可以求得的坐标。

得到坐标后，结合第i-1节的p^1i-1和坐标，可以得到θi的计算公式如下所示：

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

其中，pi表示第i个坐标系的点。pi相对于第i个坐标系的原点oi的位置关系与第一个关节上的点p1相对于o1的位置关系相同。表示从第i-1个坐标系到第i个坐标系的变换矩阵。

根据公式(6)将前面计算得到的每个关节上的靶球投影坐标换算成当前坐标系下的坐标，再将该坐标投影到该节的xoy平面上，进而得到投影直线与x轴之间的夹角

为了求解每个关节的αi和βi，需要列写传输矩阵公式。首先计算x方向以及y方向的旋转矩阵以及平移矩阵t0：

将三个矩阵相乘得到关节的变换矩阵ti：

将式(7)～(9)代入式(10)可得：

由坐标点的对应关系可以得到αi和βi的计算方式

最后，从根部开始不断循环递推得到所有的关节角。

实验验证

通过控制单段的机械臂运动，验证该方法的合理性。该实验中，设定机械臂往左右两个方向偏转，对比计算得到的关节角与理论关节角的差值，进而讨论该方法的有效性。分别根据理想的运动学模型，通过拉伸绳索控制机械臂单段(含6小节)往左偏转8.5°*6＝51°和往右偏转51°。由于该机械臂的联动效果，理想情况下每一个关节均偏转8.5°。再通过外部相机采集的坐标值，结合该算法计算出实际的每个关节的偏转角度，进而比较实际偏转角度与理论偏转角度的差异，为进一步的误差分析提供帮助。

实验结果如表1所示。

表1实际测量关节角度与理论关节角度对比

从测量得到的理论值与实际值对比来看，最大误差不超过0.3°，说明该方法可以用于检测多关节连续型机械臂关节角度，进而重建其三维臂型。

本发明的背景部分可以包含关于本发明的问题或环境的背景信息，而不一定是描述现有技术。因此，在背景技术部分中包含的内容并不是申请人对现有技术的承认。

以上内容是结合具体/优选的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，其还可以对这些已描述的实施方式做出若干替代或变型，而这些替代或变型方式都应当视为属于本发明的保护范围。在本说明书的描述中，参考术语“一种实施例”、“一些实施例”、“优选实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。尽管已经详细描述了本发明的实施例及其优点，但应当理解，在不脱离专利申请的保护范围的情况下，可以在本文中进行各种改变、替换和变更。

技术特征：

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＝oipi¹×pi¹pi²(1)

由ni垂直于向量得到第一个约束关系：

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

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

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

3.如权利要求1或2所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤s3包括：

将第i节关节的坐标结合第i-1节关节的和坐标，计算夹角θi：

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

其中，pi表示第i个坐标系的点，pi相对于第i个坐标系的原点oi的位置关系与第一个关节上的点p1相对于o1的位置关系相同，表示从第i-1个坐标系到第i个坐标系的变换矩阵；

根据公式(6)将前面计算得到的每个关节上的靶球投影坐标换算成当前坐标系下的坐标，再将该坐标投影到对应关节的xoy平面上，进而得到投影直线与x轴之间的夹角即关节的旋转角。

4.如权利要求1至3任一项所述的绳驱机械臂三维臂型测量方法，其特征在于，步骤s4包括：

计算x方向以及y方向的旋转矩阵以及平移矩阵t0：

将旋转矩阵以及平移矩阵t0相乘得到关节的变换矩阵ti：

即，将式(7)～(9)代入式(10)得到关节的变换矩阵ti：

通过式(12)表示的坐标点的对应关系计算得到αi和βi：

5.一种绳驱机械臂三维臂型测量装置，其特征在于，包括：

多个靶球；

用于固定靶球的工装，所述工装在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；

多个相机，用于跟踪拍摄靶球，其经配置使被跟踪的靶球标记点至少同时有两台相机捕捉到；

处理器，与所述相机连接，用于：根据相机拍摄的图像确定靶球的坐标；根据几何关系将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。

6.如权利要求5所述的绳驱机械臂三维臂型测量装置，其特征在于，所述多个相机包括5个相机，其中4个相机通过转接件安装在三维吊丝微重力试验系统顶部的4个顶点，第5个相机安装在高度低于所述4个顶点的1根长横梁的中部。

7.一种绳驱机械臂三维臂型测量方法，其特征在于，包括如权利要求1至4任一项所述的测量方法的步骤s2-s5。

8.一种计算机可读存储介质，其特征在于，所述存储介质包括存储的计算机程序，其中，所述计算机程序运行时由处理器执行如权利要求1-4任一项所述的测量方法的步骤s2-s5。

9.一种绳驱机械臂三维臂型测量装置，其特征在于，包括处理器和计算机可读存储介质，所述存储介质包括存储的计算机程序，其中，所述计算机程序运行时由所述处理器执行如权利要求1-4任一项所述的测量方法的步骤s2-s5。

技术总结
一种绳驱机械臂三维臂型测量方法，包括如下步骤：S1、在所述绳驱机械臂的每个关节上固定两个靶球，并使每个关节上的两个靶球与所在关节的中轴线平行；通过相机拍摄靶球，获取每个关节上的两个靶球的坐标；S2、根据几何关系，将每个关节上的两个靶球的坐标投影到对应关节的中轴线上；S3、根据每个关节上的两个靶球的坐标，确定关节之间的异面直线夹角θi以及关节的旋转角S4、根据异面直线夹角θi以及旋转角确定每个关节的十字轴方向角αi和βi；S5、根据每个关节的十字轴方向角αi和βi，从根部往末端迭代得到所述绳驱机械臂的臂型。相对于传统直接利用外侧靶球的坐标进行测量的方法，本发明可以更加准确地测出绳驱机械臂的三维臂型。

技术研发人员：孟得山;王学谦;梁斌;黄少平;米奕霖;徐文福
受保护的技术使用者：清华大学深圳国际研究生院
技术研发日：2020.01.13
技术公布日：2020.06.05