CN112720450A - 机器人关节角度检验方法、装置、设备及介质 - Google Patents

Abstract

本发明公开了一种机器人关节角度检验方法、装置、设备及介质。该机器人关节角度检验方法包括：获取目标机械臂关节上的加速度计的运动参数，所述运动参数包括加速度数据和安装角度，所述目标机械臂为同一参照系中包括至少一个关节的机械臂；根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度；获取编码器检测的所述目标机械臂关节的第二当前关节角度，所述编码器设置于减速机的电机侧和/或输出侧；根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常。

Description

机器人关节角度检验方法、装置、设备及介质

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种机器人关节角度检验方法、装置、设备及介质。

背景技术

随着人工智能技术的发展，工业机器人将有着广阔的市场应用前景。在工业机器人工作时，其关节角度与工作状态息息相关。工业机器人往往是将当前的编码器值减去设定的零点值来确定当前的关节角度，通常会在减速机的电机侧采用多圈绝对式编码器，或者在输出侧采用单圈或多圈绝对式编码器。当电机侧的多圈绝对式编码器出现圈数异常，比如记录圈数的电池松动了，或者输出侧的绝对式编码器出现绝对值丢失，比如编码器码盘被刮花，这时机器人的关节角度将出错。另一种情况是设定的零点值写错了，也会导致机器人的关节角度将出错。机器人的关节角度出错将造成重复精度的丢失，拖动示教失灵，碰撞检测判断失误，严重者将撞机。实际工作过程中，不可能每次上电都让机器人回零，然后肉眼判断机器人是否处于竖直状态，以此来判断机器人的关节角度是否正确，这样不仅难以保证检验的灵敏度，还增加额外的人工成本。

发明内容

本发明实施例提供一种机器人关节角度检验方法、装置、设备及介质，以解决关节角度检验的灵敏度不高、检验不便的问题。

第一方面，本发明实施例提供一种机器人关节角度检验方法，包括：

获取目标机械臂关节上的加速度计的运动参数，所述运动参数包括加速度数据和安装角度，所述目标机械臂为同一参照系中包括至少一个关节的机械臂；

根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度；

获取编码器检测的所述目标机械臂关节的第二当前关节角度，所述编码器设置于减速机的电机侧和/或输出侧；

根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常。

第二方面，本发明实施例提供一种机器人关节角度检验装置，包括：

第一获取模块，获取目标机械臂关节上的加速度计的运动参数，所述运动参数包括加速度数据和安装角度，所述目标机械臂为同一参照系中包括至少一个关节的机械臂；

计算模块，根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度；

第二获取模块，获取编码器检测的所述目标机械臂关节的第二当前关节角度，所述编码器设置于减速机的电机侧和/或输出侧；

判断模块，根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常。

本发明第三方面提供一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如本发明第一方面所述的机器人关节角度检验方法的步骤。

本发明第四方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如本发明第一方面所述的机器人关节角度检验方法的步骤。

本发明实施例提供的机器人关节角度检验方法，装置、设备及存储介质中，通过获取机器人关节上加速度计的加速度数据，并根据加速度计的安装角度，进行推导，得出最接近的实时关节角度，对比由编辑器获取的关节角度，能够有效地检测机器人的关节角度是否正确。该机器人关节角度检验方法成本低，计算量较小，有效地提高了关节角度检验的灵敏度，以及机器人的安全可靠性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一实施例中提供的机器人关节角度检验方法的一流程图；

图2是图1中步骤S12的一具体实施方式的一流程图；

图3是本发明一具体实施方式中提供的机器人关节角度检验装置的一原理框图；

图4是本发明一实施例中提供的计算机设备的一结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在一实施例中，如图1所示，提供一种机器人关节角度检验方法。该机器人关节角度检验方法可应用在机器人，通过机器人内部的处理器或者控制装置实现。进一步地，该机器人关节角度检验方法也可以通过外部的计算机终端实现。该计算机终端可以但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑或便携式可穿戴设备。如图1所示，该机器人关节角度检验方法包括如下步骤：

S11：获取目标机械臂关节上的加速度计的运动参数，运动参数包括加速度数据和安装角度，目标机械臂为同一参照系中包括至少一个关节的机械臂。

可以理解地，一个物体在空间中的运动状态可以通过六个自由度进行描述，分别为三个移动自由度和三个转动自由度，因此需要建立多个坐标系对其运动状态进行描述。目标机械臂为同一参照系中包括至少一个关节的机械臂，可选地，上述同一参照系为地面参照系。即当目标机械臂的关节数量超过一个时，其中一个关节的运动能够对其他关节的运动状态造成影响。可选地，以地面参照系为惯性参照系，当重力加速度在加速度计坐标系上的投影值不为零时，对于该关节而言，需要建立非惯性参照系对关节的运动状态进行描述，非惯性参照系是相对某惯性参照系作非匀速直线运动的参照系。目标机械臂的关节上设有加速度计，加速度的数量以实际测量对象为准，具体地，对于包括三个及三个以上关节的目标机械臂，加速度计的数量至少选取三个，以满足计算需求；当目标机械臂处于水平状态时，自由度减少，或关节数量少于三个时，加速度计的数量可以少于三个。优选地，设置的加速度计的数量与目标机械臂的关节数量相一致，每个关节上安装有一个加速度计。

S12：根据加速度数据和安装角度进行计算，得到目标机械臂关节的第一当前关节角度。

具体地，确定加速度与各空间坐标系的姿态变换矩阵的关系式，并建立姿态变换矩阵与加速度计的安装角度、关节角度的方程组，得到机器人目标关节臂处于水平和非水平状态下对应的关节角度表达式，对上述关节角度表达式进行多次迭代计算，获取目标机械臂关节的第一当前关节角度。

其中，加速度数据通过关节处的加速度计测得，加速度数据包括加速度计在空间坐标系中的加速度数值、加速度方向以及加速度沿各轴向的分量，其中，空间坐标系包括地面坐标系、基坐标系和以各加速度计为原点的关节坐标系。加速度计的安装角度为定值。由于每个关节的转动自由度互不相同，因此需要建立多个关节坐标系，而每个关节的移动自由度相互关联，因此，各关节坐标系可以通过姿态矩阵变换建立联系。

S13：获取编码器检测的目标机械臂关节的第二当前关节角度，编码器设置于减速机的电机侧和/或输出侧。

第二当前关节角度是通过编码器获取的，可选地，通过当前的编码器值减去设定的零点值来确定。通常情况下，在减速机的电机侧采用多圈绝对式编码器，或者在输出侧采用单圈或多圈绝对式编码器。当电机侧的多圈绝对式编码器出现圈数异常，比如记录圈数的电池松动了，或者输出侧的绝对式编码器出现绝对值丢失，比如编码器码盘被刮花，这时机器人的关节角度将出错。除此之外，当设定的零点值写错了，也会导致机器人的关节角度将出错。因此需要通过实际测得的运动参数来推导出实际的关节角度，判断关节角度对否异常。

S14：根据第一当前关节角度和第二当前关节角度，判断机器人关节角度是否异常。

第一当前关节角度是通过实际的运动参数加以理论推导计算得出的实际关节角度，第二当前关节角度是通过编码器获得的理论关节角度，二者相互对照，就能判断出机器人的关节角度情况。

本实施例提供的机器人关节角度检测方法，通过在机器人的目标机械臂上设置加速度计，最终获取当前关节角，并与由编码器获得的关节角进行对比，来判断机器人的关节角度情况。该方法不仅成本低廉，检测效率高，计算简单，更能极大地提高关节角度检验的灵敏度，保证机器人安全可靠地工作。

在一实施例中，如图2，根据加速度数据和安装角度进行计算，得到目标机械臂关节的第一当前关节角度，具体包括以下步骤：

S121：建立关节角度与运动参数的方程组。

具体地，确定加速度在以加速度计为原点建立的坐标系中沿各轴向的分量和各关节坐标系之间的姿态变换矩阵，建立等式关系，确定关于加速度的函数关系式。

S122：对所述方程组进行变换，得到关节角度关于运动参数的表达式；

加速度数据不仅包括加速度计的实际测量值，还包括理论计算的函数值，通过加速度计的实际测量加速度值和理论推导加速度值，可以得到未知角的表达式，加速度误差表达式和误差系数表达式。

S123：通过高斯-牛顿迭代法求解表达式，获取相应的第一当前关节角度。

第一当前关节角度是通过多次迭代获得，迭代次数越多，得到的关节角精度越高。通过当前关节角迭代值、当前加速度误差值和当前误差系数，可以得到更精确的关节角度值。

在本实施例中，通过对加速度数据的进一步处理，得到第一当前关节角。该方法步骤简明清晰，极大地精简了计算过程，使得整体检测方案更加地简单而高效。

在一实施例中，建立关节角度与运动参数的方程组，包括：

建立如下式关节角度与运动参数的方程组：

a_n＝(R_bR₁...R_mA_n)^TG；

其中，a_n为重力加速度在第n个加速度计坐标系上的投影值，R_b为地面坐标系x_by_bz_b到基座坐标系x₀y₀z₀的姿态变换矩阵，R₁为基座坐标系x₀y₀z₀到第一关节坐标系x₁y₁z₁的姿态变换矩阵，R_m为第m-1关节坐标系x_m-1y_m-1z_m-1到第m关节坐标系x_my_mz_m的姿态变换矩阵，A_n为与加速度计的安装角度对应相关的姿态变换矩阵，为常量矩阵，T表示矩阵的转置，G为重力加速度转置矩阵。

例如，当目标机械臂包括三个或三个以上的关节时，任选其中三个关节，分别设置对应的加速度计，第一个加速度计装在第一关节，第二个加速度计装在第三关节，第三个加速度计装在第五关节。其关节角度与运动参数的方程组为：

其中，上标T表示矩阵的转置，a₁＝[a_1x a_1y a_1z]，a₃＝[a_2x a_2y a_2z]，a₃＝[a_3x a_3ya_3z]，a_1x、a_1y、a_1z分别为第一个加速度计在坐标系x_a1 y_a1 z_a1上x、y、z方向的加速度值，a_2x、a_2y、a_2z分别为第二个加速度计在在坐标系x_a2 y_a2 z_a2上x、y、z方向的加速度值，a_3x、a_3y、a_3z分别为第三个加速度计在在坐标系x_a3 y_a3 z_a3上x、y、z方向的加速度值。G＝[0 0 g]^T，g＝9.8m/s²为重力加速度。R_b为地面坐标系x_b y_b z_b到基座x₁ y₁ z₁坐标系x₀ y₀ z₀的姿态变换矩阵，R₁为基座坐标系x₀ y₀ z₀到第一关节坐标系的姿态变换矩阵，R₂为第一关节坐标系x₁y₁ z₁到第二关节坐标系x₂ y₂ z₂的姿态变换矩阵，R₃为第二关节坐标系x₂ y₂ z₂到第三关节坐标系x₃ y₃ z₃的姿态变换矩阵，R₄为第三关节坐标系x₃ y₃ z₃到第四关节坐标系x₄ y₄ z₄的姿态变换矩阵，R5为第四关节坐标系x₄ y₄ z₄到第五关节坐标系x₅ y₅ z₅的姿态变换矩阵。A₁为第一关节坐标系x₁ y₁ z₁到第一个加速度计坐标系x_a1 y_a1 z_a1的姿态变换矩阵，A₂为第三关节坐标系x₃ y₃ z₃到第二个加速度计坐标系x_a2 y_a2 z_a2的姿态变换矩阵，A₃为第五关节坐标系x₅ y₅ z₅到第三个加速度计坐标系x_a3 y_a3 z_a3的姿态变换矩阵，A₁、A₂、A₃与加速度计的安装角度有关，为常量矩阵。

上述的姿态变换矩阵与加速度计的安装角度、关节角度相关，在一个具体实施方式中，姿态变换矩阵如下：

其中，θ₁、θ₂、θ₃、θ₄、θ₅分别为机器人的五个关节角度值，θ_b为目标机械臂与水平面的倾斜角度。

在本实施例中，通过对加速度数据与关节角度、加速度计安装角度的进一步处理，保证了检测方法更加高效。

在一实施例中，对方程组进行计算，得到运动参数对应的关节角度θ，误差系数矩阵J(θ)和加速度值误差矩阵e(θ)的表达式，包括：

当sinθ_b≠0时，

其中，

分别是第一、二、三个加速度计的实际测量值，a₁、a₂、a₃分别是第一、二、三个加速度计根据上一个实施例计算出来的值。

当sinθ_b＝0且a₂≠A₂ ^T时，

当sinθ_b＝0且a₂＝A₂ ^T时，

在本实施例中，通过对关节角度在机器人目标关节臂处于水平状态和非水平状态下以及加速度与安装角度处于特定情况的不同计算方式，更好地保证了该检测方法的准确和合理。

在一实施例中，通过高斯-牛顿迭代法求解表达式，获取相应的第一当前关节角度，具体包括：

通过如下公式求解所述表达式，获取相应的第一当前关节角度θ^(k+1)：

θ^(k+1)＝θ^(k)-[J(θ^(k))^TJ(θ^(k))]^-1J(θ^(k))^Te(θ^(k))

其中，θ为关节角度，θ^(k)为θ的第k次迭代值，J(θ^(k))为θ^(k)的误差系数矩阵，J(θ^(k))^T为θ^(k)的误差系数矩阵转置，e(θ^(k))为θ^(k)对应的加速度值误差矩阵。

在本实施例中，迭代的初值可选用第二当前关节角度。

在本实施例中，通过高斯-牛顿迭代法来得出最终的第一当前关节角，根据精度要求，可自行选择迭代次数，当精度要求不高时，可减少迭代次数，这样更高效；当精度要求较高时，可适当增加迭代次数，这样使得结果更加准确。

在一实施例中，根据第一当前关节角度和第二当前关节角度，判断机器人关节角度是否异常，具体包括：

计算所述第一当前关节角和所述第二当前关节角的误差；若所述第一当前关节角和所述第二当前关节角的误差在预设误差阈值内，则判定关节角度正常；若所述第一当前关节角和所述第二当前关节角的误差超出预设误差阈值，则判定关节角度异常。

可选地，计算第一当前关节角和第二当前关节角的误差，通过二者相减得到当前误差值，例如取当前误差值为α，将当前误差值α与预设误差阈值β进行比较,可以理解地，该误差阈值β可以为一个数值范围。

若第一当前关节角和第二当前关节角的当前误差值α在预设误差阈值β内，即α∈β，则判定关节角度正常；若第一当前关节角和第二当前关节角的当前误差值α超出预设误差阈值β，即

则判定关节角度异常。

具体地，其中第一当前关节角是通过迭代计算得出的角度，当迭代次数足够时，其数值非常接近实际值，第二当前关节角是由编码器得到的关节角，当机器人关节正常时，二关节角近似相等，当机器人关节发生异常时，二关节差值将超出预设误差阈值。

本实施例通过比较第一当前关节角和第二当前关节角的当前误差值和预设误差阈值，来判断机器人关节的工作情况，直观准确，可靠性强。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

在一实施例中，提供一种机器人关节角度检测装置，该机器人关节角度检测装置与上述实施例中机器人关节角度检测方法一一对应。如图3所示，该机器人关节角度检测装置包括第一获取模块21、计算模块22、第二获取模块23和判断模块24。各功能模块详细说明如下：

第一获取模块21，用于获取目标机械臂关节上的加速度计的运动参数，运动参数包括加速度数据和安装角度，目标机械臂为同一参照系中包括至少一个关节的机械臂；

计算模块22，用于根据加速度数据和安装角度进行计算，得到目标机械臂关节的第一当前关节角度；

第二获取模块23，用于获取编码器检测的目标机械臂关节的第二当前关节角度，编码器设置于减速机的电机侧和/或输出侧；

判断模块24，用于根据第一当前关节角度和第二当前关节角度，判断机器人关节角度是否异常。

优选地，所述加速度数据包括所述加速度计在空间坐标系中的加速度数值、加速度方向以及加速度沿各轴向的分量，其中，所述空间坐标系包括地面坐标系和以各加速度计为原点的关节坐标系。

优选地，所述计算模块22用于建立关节角度与所述运动参数的方程组；对所述方程组进行变换，得到关节角度关于所述运动参数的表达式；通过高斯-牛顿迭代法求解所述表达式，获取相应的第一当前关节角度。

优选地，所述计算模块22还用于建立如下式所述的关节角度与所述运动参数的方程组：

a_n＝(R_bR₁...R_mA_n)^TG；

其中，a_n为重力加速度在第n个加速度计坐标系上的投影值，R_b为地面坐标系x_b y_bz_b到基座坐标系x₀ y₀ z₀的姿态变换矩阵，R₁为基座坐标系x₀ y₀ z₀到第一关节坐标系x₁ y₁z₁的姿态变换矩阵，R_m为第m-1关节坐标系x_m-1 y_m-1 z_m-1到第m关节坐标系x_m y_m z_m的姿态变换矩阵，A_n为与加速度计的安装角度对应相关的姿态变换矩阵，为常量矩阵，T表示矩阵的转置，G为重力加速度转置矩阵。

优选地，所述计算模块22还用于对所述方程组进行计算，得到所述运动参数对应的关节角度θ，误差系数矩阵J(θ)和加速度值误差矩阵e(θ)的表达式。

优选地，所述计算模块22还用于通过如下公式求解所述表达式，获取相应的第一当前关节角度θ^(k+1)：

θ^(k+1)＝θ^(k)-[J(θ^(k))^TJ(θ^(k))]^-1J(θ^(k))^Te(θ^(k))；

其中，θ为关节角度，θ^(k)为θ的第k次迭代值，J(θ^(k))为θ^(k)的误差系数矩阵，J(θ^(k))^T为θ^(k)的误差系数矩阵转置，e(θ^(k))为θ^(k)对应的加速度值误差矩阵。

优选地，所述判断模块24用于计算所述第一当前关节角和所述第二当前关节角的误差；若所述第一当前关节角和所述第二当前关节角的误差在预设误差阈值内，则判定关节角度正常；若所述第一当前关节角和所述第二当前关节角的误差超出预设误差阈值，则判定关节角度异常。

本实施例提供的机器人关节角度检验装置，用于检测机器人关节角度，结构简单，检测效率高，能极大地提高关节角度检验的灵敏度，保证机器人安全可靠地工作。

关于机器人关节角度检验装置的具体限定可以参见上文中对于机器人关节角度检验方法的限定，在此不再赘述。上述机器人关节角度检验装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图4所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储上述实施例中的机器人关节角度检验方法所使用到的数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种机器人关节角度检验方法。

在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现一种机器人关节角度检验方法。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现一种机器人关节角度检验方法。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种机器人关节角度检验方法，其特征在于，所述关节上设有加速度计，所述机器人关节角度检验方法包括：

获取目标机械臂关节上的加速度计的运动参数，所述运动参数包括加速度数据和安装角度，所述目标机械臂为同一参照系中包括至少一个关节的机械臂；

根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度；

获取编码器检测的所述目标机械臂关节的第二当前关节角度，所述编码器设置于减速机的电机侧和/或输出侧；

根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常。

2.如权利要求1所述的机器人关节角度检验方法，其特征在于，所述加速度数据包括所述加速度计在空间坐标系中的加速度数值、加速度方向以及加速度沿各轴向的分量，其中，所述空间坐标系包括地面坐标系和以各加速度计为原点的关节坐标系。

3.如权利要求1所述的机器人关节角度检验方法，其特征在于，所述根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度，具体包括：

建立关节角度与所述运动参数的方程组；

对所述方程组进行变换，得到关节角度关于所述运动参数的表达式；

通过高斯-牛顿迭代法求解所述表达式，获取相应的第一当前关节角度。

4.如权利要求3所述的机器人关节角度检验方法，其特征在于，所述建立关节角度与所述运动参数的方程组，包括

建立如下式所述的关节角度与所述运动参数的方程组：

a_n＝(R_bR₁...R_mA_n)^TG；

其中，a_n为重力加速度在第n个加速度计坐标系上的投影值，R_b为地面坐标系x_by_bz_b到基座坐标系x₀y₀z₀的姿态变换矩阵，R₁为基座坐标系x₀y₀z₀到第一关节坐标系x₁y₁z₁的姿态变换矩阵，R_m为第m-1关节坐标系x_m-1y_m-1z_m-1到第m关节坐标系x_my_mz_m的姿态变换矩阵，A_n为与加速度计的安装角度对应相关的姿态变换矩阵，为常量矩阵，T表示矩阵的转置，G为重力加速度转置矩阵。

5.如权利要求3所述的机器人关节角度检验方法，其特征在于，所述对所述方程组进行变换，得到关节角度关于所述运动参数的表达式，具体为：

对所述方程组进行计算，得到所述运动参数对应的关节角度θ，误差系数矩阵J(θ)和加速度值误差矩阵e(θ)的表达式。

6.如权利要求3所述的机器人关节角度检验方法，其特征在于，所述通过高斯-牛顿迭代法求解所述表达式，获取相应的第一当前关节角度，包括：

通过如下公式求解所述表达式，获取相应的第一当前关节角度θ^(k+1)：

θ^(k+1)＝θ^(k)-[J(θ^(k))^TJ(θ^(k))]^-1J(θ^(k))^Te(θ^(k))

其中，θ为关节角度，θ^(k)为θ的第k次迭代值，J(θ^(k))为θ^(k)的误差系数矩阵，J(θ^(k))^T为θ^(k)的误差系数矩阵转置，e(θ^(k))为θ^(k)对应的加速度值误差矩阵。

7.如权利要求1所述的机器人关节角度检验方法，其特征在于，根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常，包括：

计算所述第一当前关节角和所述第二当前关节角的误差；

若所述第一当前关节角和所述第二当前关节角的误差在预设误差阈值内，则判定关节角度正常；

若所述第一当前关节角和所述第二当前关节角的误差超出预设误差阈值，则判定关节角度异常。

8.一种机器人关节角度检验装置，其特征在于，包括：

第一获取模块，用于获取目标机械臂关节上的加速度计的运动参数，所述运动参数包括加速度数据和安装角度，所述目标机械臂为同一参照系中包括至少一个关节的机械臂；

计算模块，用于根据所述加速度数据和安装角度进行计算，得到所述目标机械臂关节的第一当前关节角度；

第二获取模块，用于获取编码器检测的所述目标机械臂关节的第二当前关节角度，所述编码器设置于减速机的电机侧和/或输出侧；

判断模块，用于根据所述第一当前关节角度和所述第二当前关节角度，判断机器人关节角度是否异常。

9.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述机器人关节角度检验方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述机器人关节角度检验方法的步骤。

Images