一种光学式运动捕捉系统下的物体运动数据捕捉方法研究

蔡嘉晖，王红伟，王漪梦

(1.北方工业大学 机械与材料工程学院，北京 100041； 2.北京航天测控技术有限公司，北京 100041)

0 引言

运动捕捉技术是通过传感器获取运动物体的运动学参数信息，近年来，运动捕捉技术取得了突飞猛进的发展，逐渐成为社会的研究热点，商业化的运动捕捉设备相继被推向市场，其已被广泛应用于数字化保护、游戏、动画、人体工程学研究、模拟训练、虚拟现实等研究领域[1]。运动捕捉技术分为机械式运动捕捉、声学式运动捕捉、电磁式运动捕捉、光学式运动捕捉和基于视频的运动捕捉，其中光学式运动捕捉系统具有使用方便，运动受限较小，采样速率高等优点[2-3]，能满足多数的高速运动捕捉需求。

光学式运动捕捉系统的原理是通过红外高速摄像机确定被测量对象上布置的被动反光或主动发光的标记点在整个运动过程中的瞬时位置，从而进行下一步的运动分析[4-5]。光学式运动捕捉技术应用广泛，如人体运动分析[6-7]、飞行器的设计的控制技术[8-9]和机器人误差补偿和控制技术[10]等。在相关研究过程中，研究者需要根据研究对象设计标记点的布置方法及其计算公式从而获得物体的运动数据，然而，根据特定对象设计的运动数据捕捉方法不具有通用性，无法应用于其他研究对象的数据捕捉过程中。因此，在基于光学运动捕捉系统下的相关研究中，研究者在其前期的标记点布置和运动数据解算过程中花费了大量的时间和精力，不利于后续的研究进展，降低了研究效率。

针以上情况，本文设计了一种光学式运动捕捉系统下的物体通用运动数据捕捉方法，能有效捕捉到不同物体的运动数据，简化了物体运动分析研究中的数据获取步骤，为后续的运动分析和控制研究提供了数据基础。本文设计的运动数据捕捉方法包括标记点的布置模型和运动数据的计算公式两部分。首先标记点模型由3个标记点组成，在物体运动过程中能有效捕捉到标记点的位置信息，然后设计了一种物体的位姿计算公式，根据标记坐标计算出物体对应的位置和姿态。同时由于测量系统的测量误差及测量环境中的噪音会对测量结果产生一定的影响，因此设计了一种卡尔曼滤波方法消除了误差的影响，得到较为平滑的物体运动曲线。最后再根据滤波后的运动数据计算出物体在运动过程中对应的速度、角速度、角速度和角加速度。最后根据本题设计的物体运动数据捕捉方法进行了机器人的末端运动捕捉实验，从而验证本文提出物体的运动数据捕捉方法的正确性。

1 物体位姿捕捉

光学式运动捕捉系统基于计算机视觉原理，由多个高速摄像机从不同角度对同一标记点的拍摄图像来确定这一点在三维空间的位置信息。摄像机组以足够高的速率连续拍摄运动物体，得到图像序列中标记点的运动轨迹，通过目标物体上标记点的监视和跟踪来完成运动捕捉任务。

由于运动捕捉系统只能获得标记点的空间坐标，因此运动物体的位姿数据需要通过捕捉特定排列的标记点后分析计算获得。由此本文提出了一种针对刚性物体的通用位姿测量方法，其标记点粘贴模型由3个不共线的标记点组成，为了方便计算，同时提高测量精度，标记点排列采用如图1所示的直角三角形方式。

图1 标记点模型及其零位坐标系

(1)

(2)

由于标记点粘贴位置不确定，标记点所构向量的模长存在过大或过小问题，不利于后续的旋转矩阵求解计算，对计算结果可能造成较大的误差影响，因此需要对以上向量进行标准化处理，得到其单位向量。

(3)

记初始状态下的标准化后的标记点单位向量集合为A，运动状态下的标记点单位向量集合为B，即:

A=[αnorm1αnorm2αnorm3]

B=[βnorm1βnorm2βnorm3]

(4)

设在运动状态下物体的标记点坐标系相对与零点坐标系的旋转矩阵为R，根据旋转后向量β等于旋转前向量α乘旋转矩阵R，可得：

βi=Rαi,i=1,2,3

(5)

根据式(5)可得：

(6)

即：

R=BA-1

(7)

物体在运动捕捉过程中的空间位置可看作为3个标记点的几何中心，由此可得到在初始状态下的物体位置坐标PZ为：

(8)

设在运动状态下物体的相对于初始状态的位置坐标为P，根据当前标记点位置何物体的零位坐标PZ可得：

(9)

通过式(1)可求得的运动状态下物体坐标系相对与初始状态下零点坐标系的相对坐标为P，通过式(2)可求得运动状态下物体坐标系相对与初始状态下零点坐标系的旋转矩阵R，由此可得到物体运动过程中的运动坐标系相对于零点坐标系的齐次坐标为：

(10)

2 物体的运动参数计算

2.1 速度、加速度计算

光学式运动捕捉系统通过高速摄像机捕物体的运动信息，其捕捉频率与相机的帧数有关，帧数越高对应的捕捉数据越多，通常情况下一帧对应一组物体标记点坐标数据。设相机的捕捉帧数为F，可知每组数据之间采集时间间隔t=1/F，由于高速摄像机的拍摄帧数通常大于一百甚至几百帧，其采集时间间隔极短，一般为几毫秒，因此在每个时间间隔中物体的运动可近似看成匀加速运动。

图2 X轴物体运动示意图

物体在X轴的运动如图2所示，3个数据采集点的时间间隔相等，根据匀加速运动物体的位移公式可知：

(11)

(12)

(13)

同理，把物体的Y轴和Z轴运动数据代入到上述公式中，可求得物体的Y轴向速度和加速度为：

(14)

物体的Z轴向速度和加速度为：

(15)

设物体在各轴的初始速度v0=0，根据式(12)～(15)可逐个递推出物体在整个运动过程中各采集点的速度v和加速度a(速度v和加速度a均为矢量)。

2.2 角速度、角加速度计算

物体的姿态描述一般用欧拉角表示，欧拉角是用来确定定点转动刚体位置的一组独立角参量，由章动角ψ、旋进角ψ、自转角θ组成。欧拉角的定义很多，在工业标准中一般采用Z-X-Y转角系统，设物体运动坐标系{B}与初始状态下的零位坐标系为{A}重合，现将物体运动坐标系{B}先绕ZA旋转φ，然后绕YA轴旋转ψ，最后绕XA轴旋转θ，可得到运动坐标系{B}相对于零位坐标系{A}的旋转矩阵为：

R(φ,θ,ψ)=Rz(φ)Ry(θ)Rx(ψ)=

(16)

根据式(16)的物体旋转矩阵公式，可以求出物体在该旋转矩阵下对应姿态的欧拉角φ、θ、ψ，旋转矩阵到欧拉角的计算公式如式(17)所示：

(17)

同理，由于物理运动过程中相邻两个采集点的时间间隔极短，因此在每个采集点之间的物体转动可看成匀加速转动。在物体的第i采集点到i+1个采集点的绕X轴旋的转示意图如图3所示。

图3 物体章动角转动示意图

根据物体的旋转运动示意图，可知物体第i个采集点到第i+1个采集点的旋转角度的计算公式为：

(18)

(19)

已知匀加速转动物体的初始角速度和角加速度，可知物体在下一时刻的角速度为：

(20)

同理，把物体的绕Y轴的旋进角ψ和绕Z轴的自转角θ转动数据代入到上述公式中，可求得物体旋进角ψ的角速度和角加速度为：

(21)

物体自转角θ的角速度和角加速度为：

(22)

3 卡尔曼滤波

由于运动捕捉系统自身存在一定的测量误差，同时由于环境光的强弱和场景中的镜面反射都会对测量结果造成一定的误差影响。卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观察数据，对系统状态进行最有估计的算法，因此，本文捕捉设计了一种针对物体运动捕捉的卡尔曼滤波方法，能有效消除数据捕捉过程中的噪音和干扰的影响，得到相对平滑的物体运动捕捉数据曲线。

卡尔曼滤波分为预测和更新两个阶段，其预测阶段的时间更新方程为：

(24)

其更新阶段的测量更新方程为：

(25)

在本题中，需要对物体捕捉获得的位置坐标和姿态角分别进行卡尔曼滤波，因为各捕捉点之间的时间间隔为几到几十毫秒，因此在每个捕捉点之间物体可看作做匀速运动，因此，可以得到物体的位置预测值为：

(26)

同理物体的姿态角预测值为：

(27)

最后，通过把运动捕捉系统捕捉的物体位姿测量值和式(26)和式(27)获得的位置和姿态角预测值代入标准卡尔曼滤波方程中，可以得到滤波后的物体的运动数据曲线。

4 实验测试与分析

4.1 运动数据捕捉模型测试系统

为了测试所设计的物体运动数据捕捉模型的正确性，进行了物体运动捕捉测试系统的搭建。测试系统组成如图4所示。本题采用Vicon公司的多相机运动捕捉系统，其拍摄帧数为100帧，空间定位误差为0.1 mm。通过把模型粘贴在运动中的协作机器人的末端关节上获取的标记点的坐标信息，然后根据采集的标记点坐标数据计算出机器人末端关节的运动参数，对比机器人自身的运动参数从而验证模型算法的正确性，实验测试系统如图5所示。

图4 测试系统组成框图

图5 物体运动捕捉实验平台

4.2 实验测试过程

在机器人末端关节上粘贴如图6所示的标记点。在机器人示教器上设定5个运动路径点，控制机器人依次经过并在每个路径点停留1 s， 然后通过多相机运动捕捉系统捕捉机器人上标记点的坐标信息，代入到上述的运动数据求解算法中，计算出机器人末端关节的运动位姿变化轨迹，及其运动过程中的速度、加速度、角速度、角加速度。

图6 物体运动捕捉标记点模型

在机器人运动过程中，记录下经过5个路径点时机器人示教器上的空间位置和姿态，并通过运动捕捉获取的标记点坐标计算出机器人末端位姿，与示教器下机器人末端位姿进行对比，其结果如表1所示，其中各路径点数据中上行为示教器数据，下行为运动捕捉计算数据。从表可以看出，在同一路径点，通过标记点坐标计算出的机器人末端位姿与机器人示教器上的末端位姿一致，其误差小于1%，验证了上述本题设计的机器人位姿解算算法的正确性。

表1 路径点位姿数据对比

4.3 测试数据分析

通过物体的运动捕捉实验可以获得机器人末端标记点的运动轨迹，同时，为了消除运动捕捉系统误差和外界的噪音影响，对捕捉的运动数据进行卡尔曼滤波处理得到滤波后的标记点坐标数据，最后代入位姿计算公式中从而得到机器人的末端位姿运动轨迹，其结果如图7所示。

图7 机器人末端运动数据的卡尔曼滤波

从图7中可以看出，机器人在启动和停止阶段的运动数据存在较大的波动，其不利于后续的数据计算与分析。而通过卡尔曼滤波处理可以消除噪音与系统误差的影响，在卡尔曼滤波结果的局部放大图8中，可以看出通过卡尔曼滤波处理后，有效的消除了数据的波动范围，平滑了物体运动数据曲线，对后续的数据分析提供了良好的数据基础。

图8 滤波数据局部放大图

把通过滤波后的物体运动数据代入上面的速度和角速度计算公式中，求解出物体在运动过程中X、Y、Z轴的速度和角速度分量，其结果如图9所示，可以看出在机器人启动和停止阶段速度在零位附近有来回波动，这与机器人启动时发生的颤动一致。

图9 机器人末端速度和角速度

最后根据计算出的物体各轴向的速度和角速度曲线从而求解出物体的加速度和角加速度，其结果如图10所示。

图10 机器人末端加速度和角加速度

通过上面的实验数据分析结果，可以看出本题设计的物体运动数据捕捉模型能有效的获得物体在运动过程中的运动信息，包括物体的位置姿态、速度加速度和角速度角加速度的时间变化曲线。同时，在机器人的设定路径点处，捕捉数据与机器人的真实位姿误差小于1%，通过实验捕捉计算得到的机器人各运动数据曲线与机器人自身的运动数据曲线一致，从而验证了本文所提出的物体通用运动数据捕捉方法的正确性。

5 结束语

本文基于光学式运动捕捉系统设计了一种物体的通用运动数据捕捉方法，能有效捕捉不同被测物体的运动数据。首先由设计的标记点模型计算出物体的位置与姿态数据，再通过卡尔曼滤波处理消除误差的影响，从而获得较为平滑的物体运动曲线。然后根据滤波后的数据计算出各采集点对应的物体速度和角速度及其加速度和角加速度。最后，本题基于AUBO-I5型协作机器人进行运动捕捉实验，实验结果表明，本文设计的物体运动数据捕捉方法能有效获取机器人末端的运动信息，其获得的机器人运动数据在路径点处与真实机器人的位姿误差小于1%， 其位置、姿态、速度、加速度运动曲线与真实机器人运动曲线一致。本文设计的物体通用运动数据捕捉方法为基于光学式运动捕捉技术的研究提供了前提基础，降低了数据获取步骤的研究难度，缩短了研究时间，提高了相关的研究效率。能为后面研究者对于物体的运动数据捕捉方法研究提供一定的方法借鉴。