绳驱柔性机器人的臂形测量系统和方法

技术领域

本发明属于机器人技术领域，涉及用于绳驱柔性机器人的臂形测量系统和方法。本发明尤其涉及相机与柔性机器人的臂杆间相对位姿的测量系统和方法。

背景技术

灾害救援、环境探测、设备检修及维护等应用场景具有作业空间狭小、障碍物多等特点。在这些场景中，传统机器人难以完成任务，而运动灵活且体型纤细的绳驱柔性机器人(或绳驱柔性机械臂、操作臂)能够发挥重要作用。柔性机械臂主要通过绳索实现驱动，因绳索驱动过程中存在伸缩及与接触部位的摩擦阻力等。柔性机械臂的运动学及动力学方程较为复杂并难以准确反映其实际的特性，这导致柔性机械臂在执行任务的过程中，时常难以准确地到达预期给定的位置。

为此，有必要借助外部传感方式来获得柔性机械臂各臂杆的实际位置和姿态。目前一些机构研究通过视觉等方式测量全局相机与柔性臂间的位姿关系，但目前缺少一种高精度的测量系统解决方案，来测量相机与柔性臂间的位姿关系，导致视觉测量的方法因无法确定测量结果的精度而难以真正投入工程应用。因此，存在高精度、操作方便的柔性机械臂臂形测量系统的需求。

发明内容

本发明提供一种绳驱柔性机器人的臂形测量系统和方法，旨在至少解决现有技术中存在的技术问题之一。本发明用于实现柔性臂臂杆位姿测量、关节角度测量、柔性臂末端位姿测量、全局相机与柔性臂间相对位姿测量等诸多功能，还可以作为利用视觉的绳驱柔性机器人操作臂臂形测量方案的实验系统和方案精度验证系统。

本发明的方案用于绳驱柔性机器人，其机械臂部分包括多个臂杆，其中相邻的臂杆通过双旋转自由度的关节连接。

本发明的技术方案涉及一种臂形测量系统，包括：标定板和多个靶球；用于支撑所述的绳驱柔性机器人的支座组件；能够拆卸地安装至所述的绳驱柔性机器人的臂杆上的夹具件，该夹具件设置有多个靶球安装结构；具有多个视觉采集单元的位置测量仪；用于支撑所述的标定板的工装件，该工装件设置有多个靶球安装结构；由支架支撑的全局相机，其中：所述的全局相机的视场至少覆盖所述的绳驱柔性机器人的多个臂杆的运动空间，所述的全局相机被配置成由所述的标定板进行相机参数标定；所述的视觉采集单元布置在所述的支座组件的上方，并且每个的视觉采集单元的拍摄方位不同，以允许采集所述的夹具件上安装的靶球的视觉信号；所述的工装件和夹具件上的靶球安装结构由自动化机械加工来实现尺寸定位。

进一步，所述的夹具件被机械加工有三个靶球安装位置，其中每两个靶球安装点的连线之间垂直；

所述的工装件被机械加工有三个靶球安装位置，其中每两个靶球安装点的连线之间垂直。

进一步，所述的位置测量仪包括动捕系统或激光跟踪仪，所述的全局相机为双目相机。

进一步，所述的绳驱柔性机器人包括与所述的臂杆和关节串联的驱动箱，所述的支座组件的底部具有万向轮，所述的支座组件还包括：以横向方式支撑所述的驱动箱的支撑座；用于带动所述的支撑座运动的升降台。

本发明的技术方案还涉及一种臂形测量方法，所述的方法包括以下步骤：

A、根据所述的全局相机拍摄得到的多组标定板的图像进行相机标定，以标定所述的全局相机的内参数和外参数；

B、使所述的位置测量仪的坐标系作为基准坐标系，通过所述的位置测量仪读取绳驱柔性机器人臂杆上的夹具件上的靶球的相对于所述的基准坐标系的第一组位姿数据，通过柔性臂运动学算法迭代计算夹具件所在的臂杆的关节转角；

C、使所述的位置测量仪的坐标系作为基准坐标系，通过所述的位置测量仪读取装有所述的标定板的工装件上的靶球的相对于所述的基准坐标系的第二组位姿数据，然后根据工装件上的标定板和靶球安装结构的位置关系，计算标定板坐标系相对于相机坐标系的第三组位姿数据，从而计算所述的全局相机相对于所述的基准坐标系的第四组位姿数据；

D、根据所述的第一组位姿数据和所述的第四组位姿数据，通过坐标系转换计算所述的臂杆相对于所述的全局相机的目的位姿数据。

进一步，所述步骤B包括：

B11、通过所述的位置测量仪，采集不同臂杆的夹具件上的靶球的相对于所述的基准坐标系的多组位姿数据，计算两不同臂杆间的相对位姿关系为

其中，

B12、利用速度级逆运动学算法，解算雅可比矩阵，进而求解多个臂杆的关节角。

进一步，所述步骤B包括：

B21、将臂杆的预设关节角作为初始的迭代计算关节角；

B22、根据当前的迭代计算关节角，通过柔性臂正运动学算法计算所述的臂杆的当前计算位姿数据(P

B23、将所述的臂杆上的夹具件上的靶球的相对于所述的基准坐标系的测量位姿数据(P

B24、如果所述的位姿差(ΔP)大于预设的位姿差阈值(ε)并且迭代次数小于最大迭代次数阈值，则对当前的测量位姿数据(Pq)的通过求解雅可比矩阵计算新的关节角作为新的迭代计算关节角，然后返回所述的步骤B22。

进一步，在所述步骤B中，最多间隔三节臂杆来测量多个臂杆坐标系与所述的基准坐标系间的位姿关系；所述的基准坐标系被设置为世界坐标系。

进一步，所述步骤C包括：

C1、通过标定板位姿解算的张正友算法，计算标定板坐标系相对于相机坐标系的位姿矩阵

C2、计算全局相机的坐标系在动捕系统坐标系中的位姿矩阵：

其中，

进一步，所述的步骤D包括：通过坐标系转换计算所述的臂杆相对于所述的全局相机的位姿矩阵

本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。

与现有的技术相比，本发明具有以下的特点。

1、本发明提供了一套柔性机械臂臂形测量系统，借助于动捕系统或激光跟踪仪等位置测量设备，利用靶球、标定板等高精度测量柔性机械臂的臂形，也可测量全局相机与柔性臂臂杆间的相对位姿关系，可用于实现柔性臂关节角度测量、柔性臂末端位姿测量、全局相机与柔性臂间相对位姿测量等诸多功能，还可以作为利用视觉的绳驱柔性机器人操作臂臂形测量方案的实验系统和方案精度验证系统。

2、通过连接在柔性臂臂段上的靶球支撑座的机械结构设计，确定了靶球坐标系和柔性臂臂段坐标系间的相对位姿，利用动捕系统测量靶球的位置得到靶球坐标系在动捕系统坐标系中的位姿，进而得到柔性臂臂段在动捕系统坐标系中的位姿，通过对不同臂杆位姿的测量得到各段臂杆间的相对位姿，进而可以求得柔性臂各关节的转动角度。

3、利用动捕系统测量连接在工装件上的靶球位置，得到靶球坐标系在动捕系统中的位姿，利用相机拍摄标定板图片，通过标定板位姿解算程序解算标定板在相机坐标系中的位姿，进而可以求得相机坐标系在动捕系统中的位姿，结合柔性臂在动捕系统坐标系下的位姿，可以通过坐标转换得到柔性臂各段臂杆与相机间的相对位姿。

4、支撑座底部有万向轮，可带动柔性臂在平面上移动和转动，支座可以升降，带动柔性臂上下运动。相机固定在轻质量多自由度支架上，便于移动，其上有多段伸缩杆，保证相机能够在竖直方向上移动较大距离，支架上有多个旋转关节，可保证相机绕各轴旋转。相机和柔性臂的位置灵活可变，整个系统各个部件位置均可调，且系统精度由各部件间的测量精度保证，对系统的安装无特殊要求，便于试验系统搭建，使系统适应多种实验和场地要求。

附图说明

图1是本发明实施例中的绳驱柔性机器人操作臂的臂形测量系统的示意图。

图2是本发明实施例中所测量柔性臂的臂杆位姿的坐标系转换关系示意图。

图3是本发明实施例中的绳驱柔性机器人的四节臂杆间的DH参数的标识图。

图4是本发明实施例中所测量的相机位姿的坐标系转换关系示意图。

图5是标定板靶球和柔性臂靶球的坐标系建立方式示意图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。

参照图1至图4，根据本发明的臂形测量系统和方法用在一种绳驱柔性机器人200。所述的绳驱柔性机器人200的机械臂部分包括多个臂杆220和驱动箱210，其中相邻的臂杆220通过双旋转自由度的关节230连接，驱动箱210通过多条绳索带动臂杆220运动。其余关于绳驱柔性机械臂的细节描述可以参照本申请人在先的公开号为CN111993400A的发明公布文件。考虑到专利申请文本的节约原则，将该发明公布文件以全文引用的方式并入本文，并且可以作为本发明的技术方案的应用对象和应用场景限定。

参照图1，在一些实施例中，根据本发明的臂形测量系统包括：标定板130和多个靶球180；用于支撑所述的绳驱柔性机器人的支座组件110；能够拆卸地安装至所述的绳驱柔性机器人的臂杆220上的夹具件120，该夹具件120设置有多个靶球180安装结构；具有多个视觉采集单元151的位置测量仪150；用于支撑所述的标定板130的工装件140，该工装件140设置有多个靶球180安装结构；由支架170支撑的全局相机160。所述的全局相机160的视场至少覆盖所述的绳驱柔性机器人的多个臂杆220的运动空间。所述的视觉采集单元151布置在所述的支座组件110的上方，并且每个的视觉采集单元151的拍摄方位不同，以允许采集所述的夹具件120上安装的靶球180的视觉信号。

在一些实施例中，位置测量仪150可以是光学动捕系统或激光跟踪仪。以动捕系统为例，可以通过多个采集单元拍摄追踪多个靶球180，以获取靶球180的相对于位置测量仪150坐标系的位置和姿态，还可以追踪靶球180的运动轨迹。所述的全局相机160优选为双目相机，其可以通过标准的方格图案的标定板130进行相机参数标定。此外，可以设置多个全局相机160，分别布局在被测的柔性机器人的四周。

在一些实施例中，可以在多个臂杆220上间隔安装多个夹具件120，也可以在机器人的末端安装夹具件120，以用于测量多个臂杆220(含末端)的关节角。优选地，臂杆220上精确机械加工出安装孔，使得夹具件120安装在臂杆220的外周的中间位置。夹具件120上机械加工有靶球180固定结构，比如，通过同一个数控机床以预设的尺寸间隔和几何位置关系，精密加工出靶球180座的定位孔或安装孔，从而保证夹具件120上的靶球180安装后的位置尽量保持理论位置关系和精度。

在一些实施例中，标定板130上具有预设尺寸的几何的方格线图形或者黑白格图形。可以在工装件140上加工定位槽或者定位销孔结构，以精确安装标定板130。也可以将工装件140设置为板的形状，直接在板面印刷或者用激光加工的方式精确绘制预设尺寸的几何的方格线图形。然后保持数控机床的装夹不变，在标定板130所在的工装件140同一面的外围，通过同一个数控机床以预设的尺寸间隔和几何位置关系，精密加工出靶球180座的定位孔或安装孔，从而保证工装件140上的靶球180安装后的位置尽量保持理论位置关系和精度。

优选地，所述的夹具件120被机械加工有三个靶球180安装位置，其中每两个靶球180安装点的连线之间垂直；所述的工装件140被机械加工有三个靶球180安装位置。优选地，每两个靶球180安装点的连线之间垂直，如图5所示，圆圈表示靶球安装点，然后连线之间垂直。优选地，靶球180安装点的连线与标定板130上的方格线平行或者垂直。这里的垂直和平行的几何关系可以简化运算，还可以方便建立坐标系位姿转换关系。

继续参照图1，在一些实施例中，所述的支座组件110还包括：以横向方式支撑所述的驱动箱210的支撑座111；用于带动所述的支撑座111运动的升降台112。支座组件110的底部还可以装有万向轮，从而方便移动机器。升降台112和万向轮可以使测量系统进行机器人不同姿态和位置下的臂形测量。

此外，可以借助于支架170，相机可以自由移动和旋转。而且，整个测量系统的各个部件位置均可调，且系统精度由各部件间的测量精度保证，对系统的安装无特殊要求，便于试验系统搭建，使系统适应多种实验和场地要求。

因此，借助上述臂形测量系统可实现柔性臂关节角度测量、柔性臂末端位姿测量、相机与柔性臂间相对位姿测量等诸多功能，可以作为利用视觉的绳驱柔性机器人操作臂臂形测量方案的实验系统和方案精度验证系统。借助动捕系统或激光跟踪仪等位置测量设备，测量固连在夹具件120上3个靶球180的位置，3个靶球180间的位置关系通过夹具件120的机械加工保证，预先确定靶球180坐标系，则可得到靶球180坐标系相对于动捕系统坐标系的位姿。靶球180与柔性臂臂杆220坐标系之间的关系通过夹具件120与柔性臂间的机械结构位置关系保证，因此可以得到柔性臂臂杆220坐标系相对与动捕系统坐标系的位姿。通过对不同臂杆220的测量得到了各臂杆220间的相对位姿关系，由于相邻运动构件采用双自由度单元进行连接，因此后一运动构件转动中心坐标系相对前一运动构件转动中心坐标系的姿态由两个方向的角度旋转引起，结合所测出两相邻臂杆220的相对位姿，可求得这两方向的旋转角度，并同理求得所有关节的转动角度。对于动捕系统相机的布置，需要确保每个靶球180都能被两个或两个以上动捕系统相机观测，为此，将系统的其它部分看作一个整体，在系统中的前后左右四个方向各布置一个相机。在相机布置数目增加和各个相机间距离方向差异增强的情况下，位置测量仪150(如动捕系统)测量精度会有一定的提高。

基于上述的臂形测量系统的硬件情况，可以通过该系统配套的运算设备或者外部的计算机来实施根据本发明的臂形测量方法。在一些实施例中，所述的臂形测量方法包括以下步骤：

A、根据所述的全局相机160拍摄得到的多组标定板130的图像进行相机标定，以标定所述的全局相机160的内参数和外参数；

B、使所述的位置测量仪150的坐标系作为基准坐标系，通过所述的位置测量仪150读取绳驱柔性机器人臂杆220上的夹具件120上的靶球180的相对于所述的基准坐标系的第一组位姿数据，通过柔性臂运动学算法迭代计算夹具件120所在的臂杆220的关节转角；

C、使所述的位置测量仪150的坐标系作为基准坐标系，通过所述的位置测量仪150读取装有所述的标定板130的工装件140上的靶球180的相对于所述的基准坐标系的第二组位姿数据，然后根据工装件140上的标定板130和靶球180安装结构的位置关系，计算标定板130坐标系相对于相机坐标系的第三组位姿数据，从而计算所述的全局相机160相对于所述的基准坐标系的第四组位姿数据；

D、根据所述的第一组位姿数据和所述的第四组位姿数据，通过坐标系转换计算所述的臂杆220相对于所述的全局相机160的目的位姿数据。

在一些实施例中，所述步骤B可以包括：B11、通过所述的位置测量仪150，采集不同臂杆220的夹具件120上的靶球180的相对于所述的基准坐标系的多组位姿数据，计算两不同臂杆220间的相对位姿关系；B12、利用速度级逆运动学算法，解算雅可比矩阵，进而求解多个臂杆220的关节角。

在一些实施例中，所述步骤B可以包括：B21、将臂杆220的预设关节角作为初始的迭代计算关节角；B22、根据当前的迭代计算关节角，通过柔性臂正运动学算法计算所述的臂杆220的当前计算位姿数据；B23、将所述的臂杆220上的夹具件120上的靶球180的相对于所述的基准坐标系的测量位姿数据与所述的当前计算位姿数据计算位姿差；B24、如果所述的位姿差大于预设的位姿差阈值并且迭代次数小于最大迭代次数阈值，则对当前的测量位姿数据的通过求解雅可比矩阵计算新的关节角作为新的迭代计算关节角，然后返回所述的步骤B22。

在一些实施例中，所述步骤C可以包括：C1、通过标定板130位姿解算的张正友算法，计算标定板130坐标系相对于相机坐标系的位姿矩阵；C2、计算全局相机160的坐标系在动捕系统坐标系中的位姿矩阵。

下面参照图1至图5，以更具体的4个流程步骤实例来解释本发明的系统和方法流程。

1、双目相机标定

将标定板130放置于双目相机左右目都能完整观察的位置，用相机拍摄标定板130照片，保持相机不动，移动标定板130，拍摄二十组照片，利用张正友相机标定法标定双目相机内外参。

2、柔性臂位置测量

以动捕系统坐标系为世界坐标系，利用动捕系统测量固连在夹具件120上三个靶球180的位置。3个靶球180间的位置关系通过夹具件120的机械加工保证，预先确定靶球180坐标系如图4，则可得到靶球180坐标系相对于动捕系统坐标系的位姿，设为

通过对不同臂杆220的测量得到了各臂杆220间的相对位姿关系，由于相邻运动构件采用双自由度单元进行连接，因此后一运动构件转动中心坐标系相对前一运动构件转动中心坐标系的姿态由两个方向的角度旋转引起，结合所测出两相邻臂杆220的相对位姿，可求得这两方向的旋转角度，并同理求得所有关节的转动角度，具体求解过程如下：

通过前述过程测得第i节臂杆和第j节臂杆坐标系与动捕系统坐标系的位姿关系矩阵

因为两节臂杆220间位姿的齐次变换矩阵

6个关节角包括两种情况，从靠近根部关节开始先绕y轴旋转，再绕z轴旋转，即“PYYPPY”和先绕z轴旋转，再绕y轴旋转，即“YPPYYP”，以“PYYPPY”为例，建立DH参数如图3和表1所示，表中l为臂杆长度。

表1

关节角求解方式为利用速度级逆运动学求解，解算雅可比矩阵，进而求解关节角，具体非正式计算代码(伪代码)见表2。

表2

利用本发明的绳驱柔性机器人操作臂臂形测量系统测得的各节臂杆位姿求解各关节角，同时利用在柔性臂关节处外接的编码器测量各关节角的弯曲角度，将编码器测得的关节角作为理论值，本系统测得的关节角作为实际值，两种结果对比如表3。可以看出，最大测量误差不超过0.15°。同时，该误差中还包括编码器的测量误差，所以，本发明的方法测量精度足够高，可以作为臂形结果的实际值，也满足用于其它运动跟踪测量方案的更高的精度要求。

表3

3、相机位置测量

因为相机光圈位置无法通过机械测量的方式得到，所以借助视觉标定的方式测量，以动捕系统坐标系为世界坐标系，利用动捕系统测量固连在工装件140上的靶球180位置，则可得到靶球180坐标系相对于动捕系统坐标系的位姿，设为

4、柔性臂与相机间相对位姿测量

借助步骤2和步骤3得到了柔性臂和相机相对于动捕系统的位姿，则通过坐标系转换可以求得柔性臂臂杆相对于相机的位姿，设为

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。