CN111923040B - 一种集成陀螺仪的机械手末端空间姿态感知球及方法 - Google Patents

Abstract

本发明公开了一种集成陀螺仪的机械手末端空间姿态感知球及方法，包括外壳，所述外壳内部设有数据采集处理结构，所述数据采集处理结构包括依次相连的电子陀螺仪、数据处理CPU和数据发送蓝牙。采集传感器当前的6个位姿量，将角加速度与加速度计算成绕三轴的角度值，(8)计算得到感知球在地面坐标系下的三个坐标角度。上述技术方案通过集成并安装在小球内部的电子陀螺仪输出姿态数据，通过蓝牙等无线通讯方式将空间姿态的变化数据输出给计算机，计算机进行数据处理和存储，实现空间姿态角度、角速度、角加速度值反馈，为研究灵巧手手抓取物体和操作物体打下基础，对灵巧手的机械性能、运动性能作出评判，分析灵巧手的运动性能指标。

Description

一种集成陀螺仪的机械手末端空间姿态感知球及方法

技术领域

本发明涉及灵巧手空间姿态感知领域，尤其涉及一种集成陀螺仪的机械手末端空间姿态感知球及方法。

背景技术

随着科技的发展，机器人应用已经越来越广泛，例如机器人应用于自动装配线，自动分拣，自动焊接线等。在这些应用中，机器人末端执行机构的灵巧性要求越来越高，普通的夹持器已然不能满足自动化产线多样的需求，进而多指灵巧手的研究开始进入到研究者们的视野当中。

在多指灵巧手操作技术中，验证各手指的机械性能和验证所提出的操作指令是否被正确执行是必须要经历的一个步骤，其中检测操作过程中被抓持物体的空间姿态是一个重难点。因此，多指灵巧手在测试阶段进行末端位姿的检测可以表现灵巧手性能的重要指标，在这些系统中，物体位姿的检测是实现对被测物体操作的前提，包括末端位姿的角度、角速度、角加速度值，测试结果可以用于评定灵巧手的机械性能是否达标，操作的动作是否符合预期目标。

有资料显示，目前应用于多指灵巧手末端姿态检测的系统多为视觉动作捕捉系统，现有的视觉动作捕捉系统有VICON,NOKOV等，可以完成物体的空间姿态检测，但是由于其价格昂贵，一套完整的系统需要上百万，实用性较差。在针对小物体的检测时，视觉检测系统容易受到光线的影响，且在灵巧手的操作过程中，物体上的个别标点不可避免会被遮挡，使得视觉系统缺失标点坐标，导致动作捕捉失败，其检测的准确性就大大降低，同时视频流在处理过程中需要处理的数据量极大，处理有延时不能实时输出，且视觉中的图像匹配算法还需要完善。

中国专利文献CN109669533A公开了一种“基于视觉和惯性的动作捕捉方法、装置及系统”。动作捕捉方法包括：获取被捕捉对象的图像数据、通过视觉传感器模组得到的视觉传感器模组的第一运动数据及通过惯性测量模组得到的被捕捉对象的第二运动数据；根据图像数据和第一运动数据得到被捕捉对象的第一位姿数据；根据第二运动数据得到被捕捉对象的第二位姿数据；将第一位姿数据和第二位姿数据进行融合，得到被捕捉对象的动作信息。上述技术方案在捕捉过程中，物体上的个别标点不可避免会被遮挡，使得视觉系统缺失标点坐标，导致动作捕捉失败，其检测的准确性就大大降低。

发明内容

本发明主要解决原有的姿态检测系统易缺失标点坐标，检测准确性不足，视频处理延时的技术问题，提供一种集成陀螺仪的机械手末端空间姿态感知球及方法，通过集成并安装在小球内部的电子陀螺仪输出姿态数据，通过蓝牙等无线通讯方式将空间姿态的变化数据输出给计算机，计算机进行数据处理和存储，实现空间姿态角度、角速度、角加速度值反馈，为研究灵巧手手抓取物体和操作物体打下基础，对灵巧手的机械性能、运动性能作出评判，分析灵巧手的运动性能指标。

本发明的上述技术问题主要是通过下述技术方案得以解决的：本发明包括外壳，所述外壳内部设有数据采集处理结构，所述数据采集处理结构包括依次相连的电子陀螺仪、数据处理CPU和数据发送蓝牙。电子陀螺仪、数据处理CPU和数据发送蓝牙都安装在壳体内部的球内部型腔中，电子陀螺仪输出姿态数据，数据处理CPU接收姿态数据并处理，然后通过蓝牙等无线通讯方式将空间姿态的变化数据输出给计算机，计算机进行数据处理和存储，实现空间姿态角度、角速度、角加速度值反馈，为研究灵巧手手抓取物体和操作物体打下基础，同时，数据也可以用于分析灵巧手的运动性能指标。

作为优选，还包括电池，所述电池经过电压转换芯片与数据处理CPU相连，所述电池处于壳体内部几何中心位置。用电器件包括电子陀螺仪、数据处理CPU和数据发送蓝牙，电压转换芯片实现电池电压到系统需要电压的转换。电池处于壳体内部几何中心位置用于保持整个球体的重心位于球心位置，使球体保持稳定。

作为优选，所述的壳体包括上半球外壳、下半球外壳和球内部型腔，所述上半球外壳和下半球外壳通过卡扣固定连接，所述上半球外壳中集成有电子陀螺仪和数据处理CPU，所述下半球外壳集成有电池、电压转化芯片和数据发送蓝牙。壳体分为上半球外壳和下半球外壳，便于壳体内部器件的维修和替换。

一种集成陀螺仪的机械手末端空间姿态感知球的工作方法，包括以下步骤：

①采集当前的6个位姿量；

②计算加速度模长并对角加速度归一化处理；

③计算估计方向的重力；

④计算传感器测到的方向与参考方向的误差，并对误差值进行积分增益Ki倍；

⑤计算4个估计方向数值经过时间间隔T后的值并进行归一化处理；

⑥计算得到感知球在地面坐标系下的三个坐标角度。

作为优选，所述的步骤1中6个位姿量包括：绕X轴旋转的角加速度分量ACC_X，绕Y轴旋转的角加速度分量ACC_Y，绕Z轴旋转的角加速度分量ACC_Z，绕X轴旋转的角速度分量GYR_X，绕Y轴旋转的角速度分量GYR_Y，绕Z轴旋转的角速度分量GYR_Z。

作为优选，所述的步骤2计算加速度模长：

进行角加速度归一化处理：

ACC_X＝ACC_X/norm,

ACC_Y＝ACC_Y/norm,

ACC_Z＝ACC_Z/norm。

作为优选，所述的步骤3估计方向的重力计算：设置4个估计方向：

(q₀ q₁ q₂ q₃)^T＝(1 0 0 0)^T，

计算估计方向的重力

v_x＝2·(q₁·q₃-q₀·q₂),

v_y＝2·(q₀·q₁+q₃·q₂),

v_z＝q₀·q₀-q₁·q₁-q₂·q₂+q₃·q₃。

作为优选，所述的步骤4传感器测到的方向与参考方向的误差

对误差值进行积分增益Ki倍：

增益后的值＝增益前的值+(e_x e_y e_z)·Ki。

作为优选，所述的步骤5中4个估计方向数值经过时间间隔T后的值：

对4个估计方向进行归一化处理得到：

作为优选，所述的步骤6计算得到感知球在地面坐标系下的三个坐标角度，绕Z轴转动的角度：

绕Y轴转动的角度：

绕X轴转动的角度：

本发明的有益效果是：

1.计算机通过接收到的空间姿态数据，实时对灵巧手弯曲关节角度误差进行修正，实现末端空间姿态的精确。

2.计算机获取到的空间姿态数据可以被完整的保存，对这些数据进行科学的分析和处理，对灵巧手的机械性能，运动性能作出评判。

3.降低了产品成本，增加市场推广率。

附图说明

图1是本发明的一种剖面图。

图2是本发明的一种工作原理图。

图中1上半球外壳，2电子陀螺仪，3数据处理CPU，4电池，5球内部型腔，6电压转换芯片，7数据发送蓝牙，8下半球外壳。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：本实施例的一种集成陀螺仪的机械手末端空间姿态感知球及方法，如图1所示，包括壳体，壳体安装在机械手末端，使得感知球与灵巧手同步运动，在同步运动过程中生成的动态末端空间姿态数据。壳体内部设有球内部型腔5，球内部型腔5内设有依次相连的电子陀螺仪2、数据处理CPU3和数据发送蓝牙7。还包括电池4，电池4处于壳体内部几何中心位置，用于保持整个球体的重心位于球心位置，使球体保持稳定。电池4经过电压转换芯片6与数据处理CPU3相连。用电器件包括电子陀螺仪、数据处理CPU和数据发送蓝牙，电压转换芯片实现电池电压到系统需要电压的转换。

外壳包括上半球外壳1和下半球外壳8，所述上半球外壳1和下半球外壳8通过卡扣固定连接。壳体分为上半球外壳和下半球外壳，便于壳体内部器件的维修和替换。上半球外壳1中集成有电子陀螺仪2和数据处理CPU3，所述下半球外壳8集成有电池4、电压转化芯片6和数据发送蓝牙7。

一种集成陀螺仪的机械手末端空间姿态感知球的工作方法，如图2所示，包括以下步骤：

①采集当前的6个位姿量，包括：绕X轴旋转的角加速度分量ACC_X，绕Y轴旋转的角加速度分量ACC_Y，绕Z轴旋转的角加速度分量ACC_Z，绕X轴旋转的角速度分量GYR_X，绕Y轴旋转的角速度分量GYR_Y，绕Z轴旋转的角速度分量GYR_Z。

②计算加速度模长：

进行角加速度归一化处理：

ACC_X＝ACC_X/norm,

ACC_Y＝ACC_Y/norm,

ACC_Z＝ACC_Z/norm。

③估计方向的重力计算：设置4个估计方向：

(q₀ q₁ q₂ q₃)^T＝(1 0 0 0)^T，

计算估计方向的重力

v_x＝2·(q₁·q₃-q₀·q₂),

v_y＝2·(q₀·q₁+q₃·q₂),

v_z＝q₀·q₀-q₁·q₁-q₂·q₂+q₃·q₃。

④计算传感器测到的方向与参考方向的误差

对误差值进行积分增益Ki倍：

增益后的值＝增益前的值+(e_x e_y e_z)·Ki。；⑤计算4个估计方向数值经过时间间隔T后的值：

对4个估计方向进行归一化处理得到：

⑥计算得到感知球在地面坐标系下的三个坐标角度，

绕Z轴转动的角度：

绕Y轴转动的角度：

绕X轴转动的角度：

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

尽管本文较多地使用了陀螺仪、数据处理、位姿等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。

Claims (5)

1.一种集成陀螺仪的机械手末端空间姿态感知球，其特征在于，包括外壳，所述外壳内部设有数据采集处理结构，所述数据采集处理结构包括依次相连的电子陀螺仪(2)、数据处理CPU(3)和数据发送蓝牙(7)，还包括电池(4)，所述电池(4)经过电压转换芯片(6)与数据处理CPU(3)相连，所述电池(4)处于壳体内部几何中心位置；

一种集成陀螺仪的机械手末端空间姿态感知球的工作方法，包括以下步骤：

①采集当前的6个位姿量；

②计算加速度模长并对角加速度归一化处理；

③计算估计方向的重力；

④计算传感器测到的方向与参考方向的误差，并对误差值进行积分增益Ki倍，传感器测到的方向与参考方向的误差

对误差值进行积分增益Ki倍：

增益后的值＝增益前的值+(e_x e_y e_z)·Ki

式中a_x,a_y,a_z分别表示加速度计测到的沿x轴,y轴,z轴的加速度分量，

为从地理坐标系到物体坐标系下的转换矩阵，v_x,v_y, v _z分别表示在物体坐标系下的方向余弦值；

⑤计算4个估计方向数值经过时间间隔T后的值并进行归一化处理，4个估计方向数值经过时间间隔T后的值：

对4个估计方向进行归一化处理得到：

式中(q₀ q₁ q₂ q₃)^T为从欧拉角转换到四元数的值；GYR_X,GYR_Y,GYR_Z分别为陀螺仪绕x轴,y轴,z轴的角速度；norm是四元数的模长，为下一步的归一化处理计算的模长值；

⑥计算得到感知球在地面坐标系下的三个坐标角度，

绕Z轴转动的角度：

绕Y轴转动的角度：

绕X轴转动的角度：

atan为三角函数的反正切函数，asin为三角函数的反正弦函数。

2.根据权利要求1所述的一种集成陀螺仪的机械手末端空间姿态感知球，其特征在于，所述壳体包括上半球外壳(1)、下半球外壳(8)和球内部型腔(5)，所述上半球外壳(1)和下半球外壳(8)通过卡扣固定连接，所述上半球外壳(1)中集成有电子陀螺仪(2)和数据处理CPU(3)，所述下半球外壳(8)集成有电池(4)、电压转化芯片(6)和数据发送蓝牙(7)。

3.根据权利要求1所述的一种集成陀螺仪的机械手末端空间姿态感知球，其特征在于，所述步骤①中6个位姿量包括：绕X轴旋转的角加速度分量ACC_X，绕Y轴旋转的角加速度分量ACC_Y，绕Z轴旋转的角加速度分量ACC_Z，绕X轴旋转的角速度分量GYR_X，绕Y轴旋转的角速度分量GYR_Y，绕Z轴旋转的角速度分量GYR_Z。

4.根据权利要求3所述的一种集成陀螺仪的机械手末端空间姿态感知球，其特征在于，所述步骤②计算加速度模长：

进行角加速度归一化处理：

ACC_X＝ACC_X/norm,

ACC_Y＝ACC_Y/norm,

ACC_Z＝ACC_Z/norm。

5.根据权利要求1所述的一种集成陀螺仪的机械手末端空间姿态感知球，其特征在于，所述步骤③估计方向的重力计算：设置4个估计方向：

(q₀ q₁ q₂ q₃)^T＝(1 0 0 0)^T，

计算估计方向的重力

v_x＝2(q₁·q₃-q₀·q₂),

v_y＝2(q₀·q₁+q₃·q₂),

v_z＝q₀·q₀-q₁·q₁-q₂·q₂+q₃·q₃。

Images