CN102848389A - 基于视觉运动捕捉的机械臂标定及跟踪系统实现方法 - Google Patents

Abstract

<b/>本发明涉及基于视觉运动捕捉的机械臂关节的标定和转动角度计算的方法。本发明是一种基于视觉运动捕捉的机械臂标定和跟踪系统，所述的系统包括光学被动式运动捕捉系统，计算机主控设备和视觉软件和外接显示屏，包括如下组成模块：运动捕捉数据采集和处理模块；机械臂关节标定模块；关节角度计算模块；基于OpenGL的三维虚拟场景重现模块。本发明是一种非常实用有效的机器臂关节标定及角度计算的解决方案，具有很好的应用前景。

Description

基于视觉运动捕捉的机械臂标定及跟踪系统实现方法

技术领域

本发明涉及基于视觉运动捕捉的机械臂关节的标定和转动角度计算的方法。

背景技术

机械臂在工业生产、军事、航天、机器人等领域都有广泛的应用，机械臂运动的精度直接影响其性能。对机械臂进行反馈控制需要获取各关节的状态，包括当前关节的绝对角度位置及电机相对于初始状态运动的相对角度，采用传统的接触式检测元件(如电位计、霍尔等)虽然可以获得精确的数值，但是需要对机械臂进行改造，可能会影响其运动性能。因此采用非接触式的基于高精度高速率的视觉捕捉系统来获取机械臂和关节状态是理想的解决方案。

基于视觉的运动捕捉技术的出现可以追溯到20世纪70年代，迪士尼公司尝试通过捕捉演员的动作来使动画效果更加逼真。近年来，运动捕捉技术迅速发展，在影视动画制作、虚拟现实、3D视频游戏、运动仿真等领域均有大量运用。但目前对于运动捕捉的应用更多的强调动作的形似，对于各个关节运动的跟踪精度要求并不严苛。而且需要强调的是，目前基于视觉的运动捕捉都是对目标物体的绝对姿态进行还原(如机械臂当前各个关节的角度)，无法对目标各部分的相对运动(如机械臂各个关节相对于初始状态运动了多少角度)进行还原，而且由于目标的初始状态由于误差或者人为影响会有偏差，导致相对运动的还原更加困难。

基于此，本发明提出了一种高精度基于高速视觉的机械臂运动捕捉系统和实现方法，通过捕捉机械臂上的标志点空间位置，并引入标定环节，不仅可获得机械臂各关节世界坐标系的位置、绝对角度，还可获得高精度的各关节相对于初始位置的相对运动角度，从而对关节电机运动进行跟踪反馈和性能评估，具有广泛的应用前景。

发明内容

本发明提出了一种可以实时获取机械臂各关节在世界坐标系下位置的方法，同时能计算各关节的转角大小。

    本发明的具体技术方案如下：

     本发明是一种基于视觉运动捕捉的机械臂标定和跟踪系统，所述的系统包括光学被动式运动捕捉系统，计算机主控设备和视觉软件和外接显示屏，包括如下组成模块：

    a) 运动捕捉数据采集和处理模块；

    b）机械臂关节标定模块；

    c）关节角度计算模块；

    d) 基于OpenGL的三维虚拟场景重现模块。

本发明是一种基于视觉运动捕捉的机械臂标定和跟踪系统的实现方法，

    （1）、所述的运动捕捉数据采集和处理模块：在机械臂上设置标记点，利用视觉运动捕捉系统提供的API函数接口获取标记点的空间坐标，计算得到机械臂各个刚性部件的空间位置，数据获取帧率为200帧每秒；

    （2）、所述的机械臂关节标定模块：机械臂在长时间运行过程中各关节零位会发生偏移从而引入误差，在每次机械臂基座发生移动时，本模块结合机械臂关节的驱动命令与实时捕获的机械臂关节的位置，通过计算，将机械臂关节的局部坐标系与世界坐标系标定关联起来；

    （3）、所述的关节角度计算和数据保存模块：通过外部视觉观测推导得到机械臂关节旋转角度的方法，机械臂关节的旋转角度以当前初始位置为零位，与电机驱动一致，数据以文本格式保存为csv类型文件，数据的计算和保存帧率为200帧每秒；

（4）、所述的基于OpenGL的三维虚拟场景重现模块：根据上层实时处理结果或离线读取数据保存文件，在三维虚拟场景中实时显示或者记录回放机械臂运动，可根据用户需求切换任意视角，并输出到显示屏幕上。

本发明所述的结合机械臂关节的电机驱动命令与观测到的标记点信息对机械臂关节进行标定所采用的方法如下：

    （1）、记录初始状态下机械臂单个关节上的标记点；

    （2）、给关节上电机一组随机的转动命令，并让机械臂在目标位置保持不动，记录该组命令值，计算给定命令对应的仿射变换矩阵；

    （3）、记录该位置下机械臂关节的标记点，计算视觉捕捉系统得到的从初始状态到该位置的仿射变换矩阵；

    （4）、重复步骤（2）（3）得到若干组仿射变换矩阵；

    （5）、通过这若干组仿射变换矩阵计算得到初始状态下机械臂关节的位置；

    （6）、根据该关节的初始状态计算标定数据，对机械臂的其他关节重复步骤(1)~(5)得到各个关节的标定数据，最后将所有关节的标定数据保存为标定文档。

本发明所述的结合标定文档与实时观测到的标记点信息，计算机械臂关节的电机转角大小采用如下方法：

    （1）、从标定文档中得到标定数据；

    （2）、记录初始状态下机械臂关节的标记点；

    （3）、确定初始状态下机械臂关节的位置；

    （4）、机械臂作业时，实时捕获机械臂关节的标记点；

    （5）、解算在世界坐标系下机械臂各关节的位置，计算电机的旋转角度。

本发明所述的三维虚拟场景显示重现采用如下方法： 

（1）、三维虚拟场景显示模块通过OpenGL实现，可以在线设定虚拟场景内物体的绘制参数；

（2）、将机械臂作业的实时数据或者历史记录数据传入三维虚拟场景显示模块，可以直观地三维显示实时机械臂状态，并可根据用户需要任意切换视角，输出到显示屏幕中。

    本系统最大的特点是从外部实时获得机械臂作业时的重要参数，为机械臂的动作设计和执行评估提供条件，可以很容易地推广到其它运动项目及民用、军工等类似场景中。

    本发明具如下有益效果：

    1） 实现机械臂关节的精确标定，并能实时计算出关节各电机中的转角大小。

    2）本发明中的方法和系统可为机械臂的动作设计和动作执行评估提供基本且重要的数据支持。

    因此，本发明是一种非常实用有效的机器臂关节标定及角度计算的解决方案，具有很好的应用前景。

附图说明

图1系统硬件结构框图

图2系统的运行流程图

图3 标志点粘贴示例图

图4 标记点按模版分类图

图5 机械臂局部坐标系

图6 程序运行界面——机械臂关节标定

图7 程序运行界面——虚拟3维场景显示

具体实施方式

下面结合附图，详细描述本发明利用视觉运动捕捉系统进行机器臂关节标定和关节角解算的方法与系统的实施方式。在该实例中，本系统能对机械臂关节进行标定(给出关节的初始位置)，实时地获得关节中各电机的转角，并进行三维虚拟空间的场景重现。

    图1给出了系统的硬件结构框图，整个系统由8个以上摄像机1、图像数据采集卡2、主控计算机3和视觉软件4、以及外接显示屏5构成。摄像机以机械臂为中心、半径3~5米呈环形均匀分布，离地面高度为2~2.5米，视野交叉覆盖机械臂运动的有效区域；摄像机通过数据线与图像数据采集卡相连；图像数据采集卡通过主控机的PCI总线将采集到的数据传送到主控机标定程序中进行处理；处理结果输出到主控机的外接显示屏上进行显示。

    图2是系统的运行流程图，视觉运动捕捉系统的硬件平台搭建完成后，首先要标定系统的世界坐标系，然后为机械臂建立刚性模版模型。刚性模版即在机械臂运动的过程中几乎不发生形变的模块，如上、下机械臂等。模版上的标记点应不小于3个，标记点之间的距离应尽可能大，不同模版上的标记点的分布应不同，图3为机械臂上的标记点。系统运行时，摄像机1采集图像后经图像数据采集卡2传输到主控计算机3，标定软件利用视觉捕捉系统提供的API函数提取标记点数据，并结合机械臂的刚性模版文件将标记点按刚性模板归类，见图4。利用视觉捕捉系统的配套软件可以以CSV文件格式记录捕捉数据，包含时间戳、标记点位置、模版跟踪情况等相关信息。读取关节的离线标定文件，在机械臂第一次作业前根据当前的标记点信息生成各个关节的初始位置矩阵。在正式作业时，根据当前标记点信息和关节的初始位置矩阵，计算各关节中电机的转角大小。最后将得到的关节角信息、模版的标记点信息输入三维虚拟场景重现模块，根据用户需求显示机械臂的作业状态，并在外接的显示屏中6显示。在之后的段落中将对各个部分的实现做详细地介绍。

图5是机械臂局部坐标系。机械臂可简化成3段的刚性棍棒模型，分别是肩、肘、腕关节。在每个关节处建立局部坐标系，在标定过程中，初始状态时局部坐标系与世界坐标系平行。以下叙述中用到的符号说明如下：

----世界坐标系

----机械臂肩心坐标系

----机械臂肘部坐标系

----机械臂腕部坐标系

----标记点列在世界坐标系中的位置

----标记点列从i帧到j帧的旋转矩阵

----标记点列从i帧到j帧的仿射变换矩阵

----关节轴矢量

----关节旋转角度

       记

，表示

。

滚动(roll)的旋转矩阵为：

俯仰(pitch)的旋转矩阵为：

偏摆(yaw)的旋转矩阵为：

（1）机械臂关节标定的原理与方法

    当肩心坐标与世界坐标系平行时(下文称作标准初始状态)，肩关节上的标记点列记作

。一般初始状态下(即局部坐标系与世界坐标系不平行)，肩关节的标记点列记作

，记世界坐标系到肩关节局部坐标系的旋转矩阵为

，(考虑到仿射变换矩阵与旋转矩阵只相差了一个位置矢量，为简便起见，下文以旋转矩阵进行推导)。即有

从而可从一般初始状态得到标准初始状态的标记点列

对肘关节和腕关节同理有

其中

分别代表机械臂3个局部坐标系与世界坐标系分别平行时(标准初始状态)，肩关节、肘关节、腕关节上的标记点列，应注意到这些点列是难以直接观测得到的，本文称为“标准标记点”。

分别为一般初始状态时肩关节、肘关节、腕关节的标记点列，这些点列是可以直接观测得到的，但随机械臂的初始位置的不同而不同，下称为“初始标记点”。

分别为3关节从标准标记点到初始标记点的旋转矩阵。

       计算肩心坐标(或机械臂其他关节坐标)在世界坐标系下的姿态的方法分两步。      第一步是标定：通过视觉运动捕捉仪得到关节在一般初始状态下的初始标记点列，然后，给予该关节的电机若干组任意的角度，得到旋转矩阵组

       矩阵A描述了该关节经系列运动后相对于世界坐标系的姿态，矩阵B描述了该关节经系列运动后相对于自身局部坐标系的姿态。由此旋转矩阵组可得到

该关节从标准标记点列到初始标记点列的旋转矩阵，从而可得到该关节的标准标记点列，完成标定。

第二步，由视觉运动捕捉系统得到该关节标记点列在任意初始状态的初始标记点列，计算该关节从标准标记点列(在第一步中得到)到初始标记点列的旋转矩阵，便得到了该关节在世界坐标系下的初始姿态。

下面以肩关节为例具体的描述这一过程。利用视觉运动捕捉仪可获取肩关节第1帧(一般初始状态)里的标记点列，第n帧的标记点列

，即有

       肩关节共有3个自由度，在逻辑上按先俯仰(pitch)，后滚动(roll)，再偏摆(yaw)的次序进行。记作

       故又有

       可得

       记

，

，

。问题可归结为已知矩阵A，B，在欧式空间上求解矩阵方程

的问题。

的最小二乘求解。

记共有K组旋转矩阵

。

。

计算

 此时

是方程

满足最小二乘的解。

求解时，可先将矩阵对角化

则有

其中

    图6是机械臂关节标定界面，界面分为3个功能区域。区域a是视觉运动捕捉系统的控制区和静态场景的显示\隐藏开关。区域b是机械臂关节标定的参数控制和调整区。区域c是3维虚拟场景重现窗口，可以实时显示现在机械臂的运动状态。

（2）机械臂关节标定步骤

    机械臂上父连杆的姿态直接影响着子连杆的姿态，因此在标定时，要保持连杆的祖先连杆不动。

       以标定肩关节为例说明如下。

       肩关节有3个自由度，在电机的合理转角范围内向3个电机分别给定任意角度组合，得到若干系列的旋转矩阵组

。这里矩阵为世界坐标系下肩关节初始状态到末状态的旋转矩阵。

为肩关节局部坐标系下肩关节初始状态到末状态的旋转矩阵。

       求解方程得标准状态到一般状态的旋转矩阵

，进一步计算得肩关节的标准标记点列

。

       同理可得肘关节和腕关节的标准标记点列分别为

，

。

       将各关节的标准标记点列记录到磁盘文件中，得到标定文件。标定文件为文本文，各行分别记录一个关节的标定数据。数据格式见下表：

       （3）机械臂关节的电机角度解算

    驱动电机旋转的转角命令以电机的初始状态为零位。本发明的一个主要应用是从外部计算各关节中各电机的转角，以校验各电机是否执行到位。

       a．肩关节的角度解算。

       从标定文件中读取肩关节的标准标记点列

。

       利用视觉运动捕捉仪捕获机械臂上的所有标记点的初始位置，经散列点匹配后得到肩关节的标记点的初始位置，记作

。

由

可得到肩关节从标定位置到初始位置的旋转矩阵

。

       肩关节3个自由度动作的次序为俯仰、滚动、和偏摆。记作

由

可得

故肩关节运动的旋转矩阵

从中解出俯仰角

：

 (注：这里未考虑

的符号，导致丢失了一个解

。而该解在机械臂的实际运动中不会出现。)

滚动角：

偏摆角：

    b．肘关节的角度解算。

       物理上机械臂7个自由度的运动是同时进行的，逻辑上则按肩关节、肘关节、腕关节的次序依次进行。肘关节有2个自由度：滚动和偏摆。记作

       从标定文件中读取肘关节的标准标记点列

。

       利用视觉运动捕捉仪捕获机械臂上的所有标记点的初始位置，经散列点匹配后得到肘关节的标记点的初始位置，记作。

有

其中

为初始状态时肘关节相对于肩关节的旋转矩阵。

可得肘关节从标定位置到初始位置相对肩关节的旋转矩阵：

又肘关节的旋转可由下式表示：

由上式可得

又由式得

可得肘关节的滚动角：

偏摆角：

c．腕关节的角度解算。

       腕关节有2个自由度，依次作滚动和俯仰运动。记作

       从标定文件中读取腕关节的标准标记点列

。

       利用视觉运动捕捉仪捕获机械臂上的所有标记点的初始位置，经散列点匹配后得到腕关节的标记点的初始位置，记作

。

有

其中

为初始状态时腕关节相对于肘关节的旋转矩阵。

由上式可得

腕关节的旋转可由下式表示:

由上式可得：

又有

可得腕关节的滚动角：

俯仰角：

    （4）物体显示标定

    利用3dsMAX等软件构建3维物体的静态模型时与机械臂各关节的初始位置不同，为了得到3维模型从绘图位置到初始位置的仿射变换矩阵，程序实现了交互式的模型标定功能。

通过鼠标拾取物体(或按物体名字选中物体)，再通过键盘可控制物体在3维空间中自由移动和旋转，使其变化到标记点所确定的位置。最后记录各物体相对初始状态的仿射变换矩阵

，即完成了模型的标定工作。物体的显示标定文档为文本文件，各行分别记录一个物体的标定信息。数据格式见下表：

在动态显示某个物体时，物体的最终位置由标定位置与变换位置共同确定。图7为机械臂的执行姿态，1~4视图分别为上视图、前视图、左视图、和透视视图。

Claims (5)

1.一种基于视觉运动捕捉的机械臂标定和跟踪系统，其特征在于，所述的系统包括光学被动式运动捕捉系统，计算机主控设备和视觉软件和外接显示屏，包括如下组成模块：

    a) 运动捕捉数据采集和处理模块；

    b）机械臂关节标定模块；

    c）关节角度计算模块；

    d) 基于OpenGL的三维虚拟场景重现模块。

2.一种如权1所述的基于视觉运动捕捉的机械臂标定和跟踪系统的实现方法，其特征在于，

    （1）、所述的运动捕捉数据采集和处理模块：在机械臂上设置标记点，利用视觉运动捕捉系统提供的API函数接口获取标记点的空间坐标，计算得到机械臂各个刚性部件的空间位置，数据获取帧率为200帧每秒；

    （2）、所述的机械臂关节标定模块：机械臂在长时间运行过程中各关节零位会发生偏移从而引入误差，在每次机械臂基座发生移动时，本模块结合机械臂关节的驱动命令与实时捕获的机械臂关节的位置，通过计算，将机械臂关节的局部坐标系与世界坐标系标定关联起来；

    （3）、所述的关节角度计算和数据保存模块：通过外部视觉观测推导得到机械臂关节旋转角度的方法，机械臂关节的旋转角度以当前初始位置为零位，与电机驱动一致，数据以文本格式保存为csv类型文件，数据的计算和保存帧率为200帧每秒；

（4）、所述的基于OpenGL的三维虚拟场景重现模块：根据上层实时处理结果或离线读取数据保存文件，在三维虚拟场景中实时显示或者记录回放机械臂运动，可根据用户需求切换任意视角，并输出到显示屏幕上。

3.根据权利要求2所述的基于视觉运动捕捉的机械臂标定和跟踪系统的实现方法，其特征在于，所述的结合机械臂关节的电机驱动命令与观测到的标记点信息对机械臂关节进行标定所采用的方法如下：

    （1）、记录初始状态下机械臂单个关节上的标记点；

    （2）、给关节上电机一组随机的转动命令，并让机械臂在目标位置保持不动，记录该组命令值，计算给定命令对应的仿射变换矩阵；

    （3）、记录该位置下机械臂关节的标记点，计算视觉捕捉系统得到的从初始状态到该位置的仿射变换矩阵；

    （4）、重复步骤（2）（3）得到若干组仿射变换矩阵；

    （5）、通过这若干组仿射变换矩阵计算得到初始状态下机械臂关节的位置；

    （6）、根据该关节的初始状态计算标定数据，对机械臂的其他关节重复步骤(1)~(5)得到各个关节的标定数据，最后将所有关节的标定数据保存为标定文档。

4.根据权利要求3所述的基于视觉运动捕捉的机械臂标定和跟踪系统的实现方法，其特征在于，所述的结合标定文档与实时观测到的标记点信息，计算机械臂关节的电机转角大小采用如下方法：

    （1）、从标定文档中得到标定数据；

    （2）、记录初始状态下机械臂关节的标记点；

    （3）、确定初始状态下机械臂关节的位置；

    （4）、机械臂作业时，实时捕获机械臂关节的标记点；

    （5）、解算在世界坐标系下机械臂各关节的位置，计算电机的旋转角度。

5.根据权利要求2所述的基于视觉运动捕捉的机械臂标定和跟踪系统的实现方法，其特征在于，所述的三维虚拟场景显示重现采用如下方法： 

（1）、三维虚拟场景显示模块通过OpenGL实现，可以在线设定虚拟场景内物体的绘制参数；

（2）、将机械臂作业的实时数据或者历史记录数据传入三维虚拟场景显示模块，可以直观地三维显示实时机械臂状态，并可根据用户需要任意切换视角，输出到显示屏幕中。

Images