CN116839634A - 一种用机械臂标定陀螺仪的方法与机械臂 - Google Patents

Abstract

本申请涉及陀螺仪标定的领域，尤其是涉及一种用机械臂标定陀螺仪的方法与机械臂，其方法包括获取预设的由多个运动路段构成的运动路径；基于运动路径，控制机械臂沿运动路段运动并获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据；基于标定数据及陀螺仪补偿公式，获取最优补偿参数；基于最优补偿参数、陀螺仪运动数据以及陀螺仪补偿公式，获取陀螺仪的期望输出值。本申请具有可以根据需求自行选择静止姿态数据和陀螺仪运动数据的数量，以便提高陀螺仪精度的效果。

Description

一种用机械臂标定陀螺仪的方法与机械臂

技术领域

本申请涉及陀螺仪标定的领域，尤其是涉及一种用机械臂标定陀螺仪的方法与机械臂。

背景技术

MEMS陀螺仪是微机械陀螺仪，被广泛应用于自主导航、自动控制以及姿态测量等领域。为了提高陀螺仪的精准度，陀螺仪在出厂前需要进行标定，通过设置补偿参数修正陀螺仪输出从而提高陀螺仪的精度。

相关技术中，对陀螺仪标定主要采用的三轴标定转台对陀螺仪进行标定，并根据解析法求补偿参数。

针对上述中的相关技术，三轴标定转台属于专用设备，机械精度要求高且价格昂贵，而且通过三轴标定转台解析补偿参数时，补偿参数数量有限制，补偿参数数量的限制会导致其补偿精度有限。

发明内容

为了提高补偿精度，本申请提供一种用机械臂标定陀螺仪的方法与机械臂。

本申请提供的一种用机械臂标定陀螺仪的方法采用如下的技术方案：

一种用机械臂标定陀螺仪的方法，包括：

获取预设的由多个运动路段构成的运动路径；

基于运动路径，控制机械臂沿所述运动路段运动并通过在机械臂上的陀螺仪获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据；

基于标定数据及陀螺仪补偿公式，获取最优补偿参数；

基于所述最优补偿参数、所述陀螺仪运动数据以及陀螺仪补偿公式，获取陀螺仪的期望输出值。

通过采用上述技术方案，获取运动路径，然后控制机械臂沿运动路径运动，在运动的过程中获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据，将获取的标定数据结合陀螺仪基础补偿公式，即可获取满足需求的补偿后的陀螺仪的期望输出值。利用简单的机械臂，获取陀螺仪在机械臂运动过程中产生的静止姿态数据以及陀螺仪运动数据，求解出最优补偿参数以提高陀螺仪的精度，可以根据需求自行选择静止姿态数据和陀螺仪运动数据的数量，以便提高陀螺仪的精度。

可选的，所述获取路段两端静止时刻的陀螺仪的静止姿态数据包括：

当机械臂为无编码器反馈的机械臂时，控制机械臂按照预设路段运动，路段两端静止时刻用装载在机械臂上与陀螺仪对齐安装的三轴加速度计的输出值计算陀螺仪静止时的俯仰角和横滚角；

判断所述装载在机械臂上陀螺仪静止时刻的俯仰角和横滚角数值是否在该路段的预设角度区域；

若所述陀螺仪的仰角和滚角不在所述预设角度区域，则微调控制机械臂使陀螺仪的所述俯仰角和所述横滚角在所述预设角度区域以获取路段两端静止时刻的陀螺仪的静止姿态数据。

通过采用上述技术方案，机械臂分无编码器机械臂和有编码机械臂，使用无编码机械臂的时候，判断俯仰角和横滚角数值是否在预设角度区域时为了避免高仰角误差，存在高仰角误差的时候获取的数据是不准确的，在预设角度区域的时候，通过加速度计输出值计算出机械臂静止姿态角。根据不同的机械臂类型，合理选用计算机械臂静止姿态角的计算方法，避免产生误差。

可选的，所述获取路段两端静止时刻的陀螺仪的静止姿态数据还包括：

当机械臂为有编码器反馈的机械臂时，获取预设航向；

控制所述机械臂将装载在机械臂上陀螺仪与所述预设航向对齐后获取陀螺仪静止姿态数据的航向角；

通过加速度计输出值计算出陀螺仪静止姿态角的俯仰角和横滚角，将所述俯仰角、所述横滚角和所述航向角作为所述静止姿态数据。

通过采用上述技术方案，机械臂与编码器反馈的时候，编码器反馈的数据本身就是较为精准的，只要控制机械臂与预设航向对齐以便获取静止姿态数据的航向角，与预设航向对齐后如果根据编码器反馈数据方位误差小于预设误差，则可以利用加速度计计算出剩余的俯仰角和横滚角。

可选的，所述获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据包括：

获取机械臂的第i静止姿态；

基于所述运动路径，获取静止姿态数量j；

在静止时间后，控制机械臂沿所述运动路径运动至第i+1静止姿态；

获取所述第i静止姿态到所述第i+1静止姿之间的所有的所述陀螺仪运动数据以及所述静止姿态数据作为标定数据，其中，1≤i≤j-1。

通过采用上述技术方案，机械臂从最开始的静止姿态会沿运动路径开始运动，运动到下一个姿态的时候会在静止时间之后继续运动，共有j个静止姿态，机械臂从一个静止姿态运动到下一个静止姿态的时候，获取运动期间所有的陀螺仪运动数据包括两个静止姿态的陀螺仪运动数据，一直到将整个运动路径运动完毕后，收集了整个运动路径中所有的陀螺仪运动数据以及静止姿态数据，便于后续根据陀螺仪运动数据以及静止姿态数据计算最优补偿参数。

可选的，基于所述静止姿态数据，获取静止姿态向量；

基于所述静止姿态向量，构建每运动路段的初始姿态向量以及终止姿态向量；

基于所述初始姿态向量，获取初始姿态矩阵；

基于所述第i静止姿态到所述第i+1静止姿之间的所有的所述陀螺仪运动数据，以所述第i静止姿态为参考起点以及预设算法，获取由陀螺仪运动数据计算的终止姿态矩阵；

基于所述终止姿态矩阵及所述预设算法，获取陀螺仪运动数据计算的终止姿态向量；

基于所述加速度计算的终止姿态向量及陀螺仪运动数据计算的运动终止姿态向量，获取第i路段的姿态角差值；

判断所述姿态角差值是否满足角度差值要求；

若所述姿态角差值满足所述角度差值要求，则基于标定数据及陀螺仪补偿公式，获取最优补偿参数。

通过采用上述技术方案，根据静止姿态向量和陀螺仪运动数据构建出的初始姿态矩阵和终止姿态矩阵，此时的数据是没有经过补偿的，得到终止姿态矩阵的数据可能不够精准，因此在根据机械臂运动过程中采集的陀螺仪运动数据，结合预设算法，计算出计算终止姿态向量，从而获取每个路段的姿态角差值，确定出陀螺仪测量精准度，如果姿态角差值满足角度差值要求，则继续控制继续臂继续运动，根据标定数据获取最优补偿参数。在数据采集过程中，保证陀螺仪输出的数据是准确的。

可选的，所述基于标定数据及陀螺仪补偿公式，获取最优补偿参数包括：

基于所述补偿参数以及所述标定数据，计算所述运动路径包含的全部路段的姿态角误差和；

获取最小的所述姿态角误差和；

基于最小的所述姿态角，获取对应的补偿参数作为所述最优补偿参数。

通过采用上述技术方案，将所有的补偿参数结合标定数据，计算出运动路径中每一组补偿参数的姿态角误差和，然后从中找出使姿态角误差和最小的一组补偿参数即为最优补偿参数。

可选的，所述获取最小的所述姿态角误差和包括：

基于所述陀螺基础补偿公式，获取补偿参数；

获取所述补偿参数的数值范围、数值间隔以及抽样点数；

基于所述补偿参数的数值范围、数值间隔以及抽样点数，获取所有所述补偿参数集合；

基于所述补偿参数集合以及所述姿态误差公式，获取最小的所述姿态角误差和。

通过采用上述技术方案，多种补偿参数对陀螺仪进行补偿，多种不同的参数一起补偿的时候构成一组补偿参数，而每一种补偿参数有不同的数值范围，确定好数值间隔以及每种参数的抽样点数，得到补偿参数合集，将补偿参数合集中所有的参数都带入姿态误差公式中获取最小角度向量和。

可选的，所述获取最小的所述姿态角误差和之后包括：

判断所述最小的所述姿态角误差和是否满足预设要求；

若所述最小的所述姿态角误差和不满足所述预设要求，则修改所述数值范围以及所述数值间隔直至最小的所述姿态角误差和满足所述预设要求。

通过采用上述技术方案，计算出最小姿态角误差和之后，如果最小姿态角误差和的精度不满足精度要求，则需要调整数值范围以及数值间隔，重复进行计算直到满足要求。

第二方面，本申请提供一种计算机可读存储介质，采用如下的技术方案：

一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器加载并执行时，采用了上述的一种用机械臂标定陀螺仪的方法。

通过采用上述技术方案，通过将上述的一种用机械臂标定陀螺仪的方法生成计算机程序，并存储于计算机可读存储介质中，以被处理器加载并执行，通过计算机可读存储介质，方便计算机程序的可读及存储。

综上所述，本申请包括以下至少一种有益技术效果：

获取运动路径，然后控制机械臂沿运动路径运动，在运动的过程中获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据，将获取的标定数据结合托逻辑基础补偿公式，即可获取满足需求的补偿后的陀螺仪的期望输出值。利用简单的机械臂，获取陀螺仪在机械臂运动过程中产生的静止姿态数据以及陀螺仪运动数据，求解出最优补偿参数以提高陀螺仪的精度，可以根据需求自行选择静止姿态数据和陀螺仪运动数据的数量，以便提高陀螺仪的精度。

附图说明

图1是本申请实施例的机械臂的整体结构图；

图2是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图3是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图4是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图5是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图6是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图7是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图8是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图；

图9是本申请实施例的一种用机械臂标定陀螺仪的方法的其中一种实施方式的流程示意图。

附图标记说明：

1、底座；2、固定座；3、旋转座；4、第一关节、5、第二关节；6、第三关节；7、夹持件；8、加速度计；9、陀螺仪。

具体实施方式

以下结合全部附图对本申请作进一步详细说明。

参照图1，一种机械臂，包括：底座1、臂体和夹持件和6个驱动电机，底座1包括固定座2和旋转座3，固定座2上设有用于驱动旋转座3转动的第一驱动电机，臂体包括第一关节4、第二关节5和第三关节6，第一关节4与旋转座3之间设有用于驱动第一关节4转动的第二驱动电机，第一关节4与第二关节5之间设有用于驱动第二关节5转动的第三驱动电机，第二关节5与第三关节6之间设有用于驱动第三关节6转动的第四驱动电机，第三关节6与夹持件之间设置有用于驱动夹持件转动的第五驱动电机，夹持件用于夹持陀螺仪，夹持件上设有用于驱动夹持件调整张开角度的第六驱动电机，第一关节4、第二关节5以及第三关节6均转动至竖直位置时呈一条直线，夹持件调节不同地张开角度以便夹持不同类型的陀螺仪，陀螺仪上设有加速度计，陀螺仪需要与加速度计平行对齐，陀螺仪用于采集机械 臂运动过程中陀螺仪的运动数据，加速度计用于采集机械臂运动过程中的陀螺仪的静止姿态数据，为了防止运动过程中发生碰撞，驱动电机控制机械臂运动的时候，设置有机械臂最大的旋转角度，最大旋转角度可根据实际情况自行设置。

本申请实施例公开一种用机械臂标定陀螺仪的方法，参照图2，包括：

S100、获取预设的由多个运动路段构成的运动路径。

具体的，运动路径是将机械臂从初始状态运动到目标状态时所规划的运动路径，运动路径可以包括多个中间点姿态，即机械臂从初始状态运动到若干中间状态后最终运动到目标状态。

S110、基于运动路径，控制机械臂沿运动路段运动并通过在机械臂上的陀螺仪获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据。

具体的，机械臂在运动过程中包括静止和运动两个状态，机械臂静止姿态数据为机械臂在静止状态下各个关节的角度数据。在机械臂控制中，静止姿态数据通常用于描述机械臂初始状态或目标状态。陀螺仪的静止姿态数据在计算最优补偿参数的时候作为参考值。静止姿态数据通过加速度计获取，陀螺仪运动数据为装载在机械臂上的陀螺仪沿运动路径运动过程中，陀螺仪采集的数据。标定数据包括静止姿态数据和陀螺仪运动数据，通过标定数据修正陀螺仪数据使陀螺仪数据更加准确。

S120、基于标定数据及陀螺仪补偿公式，获取最优补偿参数。

具体的，最优补偿参数为防止陀螺仪输出的角度率不准确，通过最优补偿参数对陀螺仪输出的原值数据也就是采集的陀螺仪运动数据进行补偿，从而使陀螺仪运动数据更加准确。根据静止姿态数据，将所有的补偿参数的取值都带入计算一次，从中选择误差最小的一组补偿参数作为最优补偿参数。

S130、基于最优补偿参数、陀螺仪运动数据以及陀螺仪补偿公式，获取陀螺仪的期望输出值。

具体的，陀螺仪的期望输出值为在对获取的陀螺仪运动数据结合陀螺仪基础补偿公式，然后将最优补偿参数代入陀螺仪基础补偿公式中计算出陀螺仪的期望输出值，陀螺仪的期望输出值为陀螺仪运动数据经过补偿后得到的更加准确的数据。

本申请实施方式的实施原理为：获取运动路径，然后控制机械臂沿运动路径运动，在运动的过程中获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据，将获取的标定数据结合陀螺仪基础补偿公式，即可获取满足需求的补偿后的陀螺仪的期望输出值。利用简单的机械臂，获取陀螺仪在机械臂运动过程中产生的静止姿态数据以及陀螺仪运动数据，求解出最优补偿参数以提高陀螺仪的精度，可以根据需求自行选择静止姿态数据和陀螺仪运动数据的数量，以便提高陀螺仪的精度。

在本实施例的其中一种实施方式中，如图3所示，步骤S110即获取路段两端静止时刻的陀螺仪的静止姿态数据包括：

S200、当机械臂为无编码器反馈的机械臂时，控制机械臂按照预设路段运动，路段两端静止时刻用装载在机械臂上与陀螺仪对齐安装的三轴加速度计的输出值计算陀螺仪静止时的俯仰角和横滚角。

具体的，陀螺仪的静止姿态数据指的是陀螺仪静止状态下的陀螺仪角度，包括俯仰角、横滚角和航向角。俯仰角是机械臂绕其横轴旋转时与水平面之间的夹角，以XYZ轴三维坐标系来看，俯仰角即为绕X轴旋转的角度。

S210、判断装载在机械臂上陀螺仪静止时刻的俯仰角和横滚角数值是否在该路段的预设角度区域。

具体的，为了避免高仰角误差陀螺仪的俯仰角和横滚角不能设置为±90°，因此预设角度区域为除开正负90度角的其他角度区域，如果不是预设角度区域，则执行步骤S220，如果是预设角度区域，则不动作。

S220、若陀螺仪的仰角和滚角不在预设角度区域，则微调控制机械臂使陀螺仪的俯仰角和横滚角在预设角度区域以获取路段两端静止时刻的陀螺仪的静止姿态数据。

具体的，通过加速度计获取机械臂的静止姿态数据中的俯仰角、横滚角和航向角通常采用的是姿态解算算法，常用的算法包括加速度计积分法、卡尔曼滤波法、四元数法等。在这些算法中，加速度计通常用于测量机械臂末端执行器的重力加速度分量，得到机械臂在三个轴向的加速度值，通过加速度计积分法或卡尔曼滤波法等姿态解算算法，将加速度值转换为机械臂的姿态角度，测量机械臂在静止状态下重力加速度分量即可计算出机械臂静止姿态角，加速度计计算的静止姿态角中的X轴分量和Y轴分量误差要求小于0.01°。

本实施方式的实施原理为：机械臂分无编码器机械臂和有编码机械臂，使用无编码机械臂的时候，判断俯仰角和横滚角数值是否在预设角度区域时为了避免高仰角误差，存在高仰角误差的时候获取的数据是不准确的，在预设角度区域的时候，通过加速度计输出值计算出机械臂静止姿态角。根据不同的机械臂类型，合理选用计算机械臂静止姿态角的计算方法，避免产生误差。

在本实施例的其中一种实施方式中，如图4所示，步骤S110即获取路段两端静止时刻的陀螺仪的静止姿态数据还包括：

S300、当机械臂为有编码器反馈的机械臂时，获取预设航向。

具体的，预设航向为标定精度的已知方位，标定精度的已知方位为在机械臂的坐标系中已知其准确方向的参考物或者位置。这些参考物或位置可以通过各种手段进行测量和确定，例如使用GPS定位仪器。

机械臂姿态角包括X轴分量的Roll角（俯仰角）、Y轴分量的Pitch角（横滚角）和Z轴分量的Yaw角（航向角）。

S310、控制机械臂将装载在机械臂上陀螺仪与预设航向对齐后获取陀螺仪静止姿态数据的航向角。

具体的，陀螺仪与预设航向对齐时的方位误差不能过大，方位误差为机械臂与预设方位对齐的时候所允许的最大误差，通常为0.1°

将机械臂与预设航向对齐后，通过测量机械臂当前的Yaw角度，可以确定机械臂当前与参考方向之间的方位误差，方位误差为测量的Yaw角与已知方位的Yaw角的差值。

S320、通过加速度计输出值计算出陀螺仪静止姿态角的俯仰角和横滚角，将俯仰角、横滚角和航向角作为静止姿态数据。

具体的，通过加速度计计算静止姿态角的方法与步骤S220中的计算方法相同。通过有编码器的输出角度值，可以计算出下一姿态的X轴分量的Roll角、Y轴分量的Pitch角和Z轴分量的Yaw角，依次计算可以得到运动轨迹中静止姿态的全部姿态角作为参考值。

本实施方式的实施原理为：机械臂与编码器反馈的时候，编码器反馈的数据本身就是较为精准的，只要控制机械臂与预设方位对齐，机械臂的静止姿态角包括X轴、Y轴和Z轴三个方向的分量，对齐后如果根据编码器反馈数据方位误差小于预设误差，则可以利用加速度计计算出剩余的X轴分量和Y轴分量。

在本实施例的其中一种实施方式中，如图5所示，步骤S120即获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据包括：

S400、获取机械臂的第i静止姿态。

具体的，机械臂的第i静止姿态为机械臂在静止过程中的其中一个静止姿态。

S410、基于运动路径，获取静止姿态数量j。

具体的，静止姿态数量为机械臂从初始状态运行到终止状态过程中所有停留过的机械臂姿态，包括初始状态和终止状态，例如机械臂沿X轴每次转动45°，则转一圈共有8个方向，同理，沿Y轴也有8个方向以及沿Z轴也有8个方向，而机械在空间运动的时候每次转45°的时候会产生8段运动，机械臂从一个静止姿态运动到另一个静止姿态为一段运动，一共就是(8+8+8)*8=192种运动姿态，运动姿态数量即为192，每次的旋转角度可以自行设置，根据转动角度不同，运动姿态数量也就不同。

S420、在静止时间后，控制机械臂沿运动路径运动至第i+1静止姿态。

具体的，机械臂的运动包括运动和静止两个状态，静止时间为机械臂从一个静止姿态运动到下一个静止姿态后，在下一个静止姿态的停止时间，假设静止时间设置为5秒，机械臂从第一静止姿态运动到第二静止姿态后，在第二运动姿态停留5秒后，从第二静止姿态运动到第三静止姿态，以此类推得。

S430、获取第i静止姿态到第i+1静止姿态之间的所有的陀螺仪运动数据以及静止姿态数据，其中，1≤i≤j-1。

具体的，陀螺仪运动数据以及静止姿态数据是采集的两个相邻的静止姿态之间所有的陀螺仪运动数据以及静止姿态数据，包括第i静止姿态和第i+1静止姿态这两个静止姿态本身的数据，将机械臂从初始状态到最后一个运动姿态之间的所有的陀螺仪运动数据和静止姿态数据采集，即可得到机械臂在全空间范围内运动时的陀螺仪运动数据和静止姿态数据。

本实施方式的实施原理为：机械臂从最开始的静止姿态会沿运动路径开始运动，运动到下一个姿态的时候会在静止时间之后继续运动，共有j个静止姿态，机械臂从一个静止姿态运动到下一个静止姿态的时候，获取运动期间所有的陀螺仪运动数据包括两个静止姿态的陀螺仪运动数据，一直到将整个运动路径运动完毕后，收集了整个运动路径中所有的陀螺仪运动数据以及静止姿态数据，便于后续根据陀螺仪运动数据以及静止姿态数据计算最优补偿参数。

在本实施例的其中一种实施方式中，如图6所示，步骤S130即基于静止姿态数据，获取最优补偿参数包括：

S500、基于静止姿态数据，获取静止姿态向量。

具体的，静止姿态向量为根据静止姿态数据分解出来的姿态角向量数据，也就是俯仰角、横滚角和航向角。

S510、基于静止姿态向量，构建每运动路段的初始姿态向量以及终止姿态向量。

具体的，初始姿态向量为机械臂静止的时候对应的姿态向量，终止姿态向量为相对于静止姿态的下一个静止姿态的姿态向量。

S520、基于初始姿态向量，获取初始姿态矩阵。

具体的，初始姿态矩阵由初始姿态向量构成的矩阵，初始姿态矩阵为一个1*3的矩阵，初始姿态矩阵的数据为机械臂的Roll角、Y轴分量的Pitch角和Z轴分量的Yaw角组成。

S530、基于第i静止姿态到第i+1静止姿之间的所有的陀螺仪运动数据，以第i静止姿态为参考起点以及预设算法，获取由陀螺仪运动数据计算的终止姿态矩阵。

具体的，终止姿态矩阵则是通过初始姿态矩阵以及利用每段机械臂运动过程中从开始运动到静止之间的所有陀螺值根据预设算法计算出的终止姿态矩阵，预设算法可以为旋转矢量积分法，终止姿态矩阵的计算公式为：

，

其中，函数根据不同场景可设置为不同的积分方法，为初始姿态矩阵， 为通过计算得到的计算终止姿态矩阵，、以及分别为X轴方向、Y轴方向和Z轴方 向的陀螺仪值。

S540、基于终止姿态矩阵及预设算法，获取陀螺仪运动数据计算的终止姿态向量。

具体的，终止姿态向量为根据终止姿态矩阵得到的陀螺仪的俯仰角、横滚角和航向角。

S550、基于加速度计算的终止姿态向量及陀螺仪运动数据计算的运动终止姿态向量，获取第i路段的姿态角差值。

具体的，姿态角差值=，其中/>为运动终止姿态向量，/>为终止姿态向量。

，

其中，为机械臂的Roll角，/>为机械臂的Pitch角。

S560、判断姿态角差值是否满足角度差值要求。

具体的，角度差值为判断陀螺仪的测量精度和稳定性的数据，在陀螺仪正常的情况下，预设差值通常接近于0，可以根据产品的需求自行允许的预设差值数值。

S570、若姿态角差值满足角度差值要求，则基于标定数据及陀螺仪补偿公式，获取最优补偿参数。

具体的，只有在确认陀螺仪本身没有问题的情况，采集的陀螺仪运动数据才有意义，因此需要先确认陀螺仪正常，之后再根据静止姿态数据和陀螺仪运动数据获取最优补偿参数。

本实施方式的实施原理为：根据静止姿态向量和陀螺仪运动数据构建出的初始姿态矩阵和终止姿态矩阵，此时的数据是没有经过补偿的，得到终止姿态矩阵的数据可能不够精准，因此在根据机械臂运动过程中采集的陀螺仪运动数据，结合预设算法，计算出计算终止姿态向量，从而获取每个路段的姿态角差值，确定出陀螺仪测量精准度，如果姿态角差值满足角度差值要求，则继续控制继续臂继续运动，根据标定数据获取最优补偿参数。在数据采集过程中，保证陀螺仪输出的数据是准确的。

在本实施例的其中一种实施方式中，如图7所示，步骤S570即基于静止姿态数据，获取最优补偿参数包括：

S600、基于补偿参数、陀螺仪数据以及姿态误差公式，计算运动路径包含的全部路段的姿态角误差和。

具体的，姿态误差公式为相邻两个姿态之间的姿态差值，姿态误差公式为：

，

具体的，姿态角误差和为。

S610、获取最小的姿态角误差和。

具体的，姿态误差公式中包括了多种补偿参数，而每一种补偿参数都有各自的取值范围，将取值范围内的补偿参数都带入姿态误差公式中得到的最小姿态角误差和所对应的一组补偿参数即为最优补偿参数。

S620、基于最小的姿态角，获取对应的补偿参数作为最优补偿参数。

本实施方式的实施原理为：将所有的补偿参数结合标定数据，计算出运动路径中每一组补偿参数的姿态角误差和，然后从中找出使姿态角误差和最小的一组补偿参数即为最优补偿参数。

在本实施例的其中一种实施方式中，如图8所示，步骤S710即获取最小的姿态角误差和包括：

S700、基于陀螺基础补偿公式，获取补偿参数。

具体的，陀螺仪基础补偿公式为：

，

其中，为陀螺仪的期望输出值，/>为一个3*3的矩阵，K为标度因数参数、A为正交参数，矩阵中的数据为补偿参数的一部分，/>为零点参数，也是补偿参数，因此，一共有12个补偿参数，/>为陀螺仪的输出值。

S710、获取补偿参数的数值范围、数值间隔以及抽样点数。

具体的，数值范围为补偿参数的取值范围，所有的补偿参数都有一定的取值范围， 数值间隔为每个参数相邻两个抽样点之间的间隔，抽样点数=数值范围/数值间隔+1，例如， 补偿参数个数为12，每个参数抽样点数为21，可得到每组参数为12，数量为组的补偿参 数集合。

S720、基于补偿参数的数值范围、数值间隔以及抽样点数，获取所有补偿参数集合。

S730、基于补偿参数集合以及姿态误差公式，获取最小的姿态角误差和。

具体的，将集合内的补偿参数逐一代入姿态误差补偿公式中，使最小的一组补偿参数，即为当前数值间隔下的最优补偿参数。

本实施方式的实施原理为：多种补偿参数对陀螺仪进行补偿，多种不同的参数一起补偿的时候构成一组补偿参数，而每一种补偿参数有不同的数值范围，确定好数值间隔以及每种参数的抽样点数，得到补偿参数合集，将补偿参数合集中所有的参数都带入姿态误差公式中获取最小的姿态角误差和。

在本实施例的其中一种实施方式中，如图9所示，步骤S730即获取最小的姿态角误差和之后包括：

S800、判断最小的姿态角误差和是否满足预设要求。

具体的，陀螺仪在计算最小的姿态角误差和的时候，根据数值范围选取的不同以及数值间隔设置的不同，所计算出来的最小的姿态角误差和不同，补偿后陀螺仪的精度自然就不相同，预设要求即为补偿后的陀螺仪精度是否满足出厂要求，出厂要求根据不同的需求不相同。

S810、若最小的姿态角误差和不满足预设要求，则修改数值范围以及数值间隔直至最小的姿态角误差和满足预设要求。

具体的，最小的姿态角误差和不满足预设要求的时候，需要降低数值范围和减小数值间隔，并重新执行实施例8的过程，计算出最小角度向量以提高补偿精度。

本实施方式的实施原理为：计算出最小的姿态角误差和之后，如果最小的姿态角误差和精度不满足要求，则需要调整数值范围以及数值间隔，重复进行计算直到满足要求。

以上均为本申请的较佳实施例，并非依此限制本申请的保护范围，故：凡依本申请的结构、形状、原理所做的等效变化，均应涵盖于本申请的保护范围之内。

Claims (9)

1.一种用机械臂标定陀螺仪的方法，其特征在于，包括：

获取预设的由多个运动路段构成的运动路径；

基于所述运动路径，控制机械臂沿所述运动路段运动并获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据；

基于所述标定数据及陀螺仪补偿公式，获取最优补偿参数；

基于所述最优补偿参数、所述陀螺仪运动数据以及所述陀螺仪补偿公式，获取陀螺仪的期望输出值。

2.根据权利要求1所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述获取路段两端静止时刻的陀螺仪的静止姿态数据包括：

当机械臂为无编码器反馈的机械臂时，控制机械臂按照预设路段运动，路段两端静止时刻用装载在机械臂上与陀螺仪对齐安装的三轴加速度计的输出值计算陀螺仪静止时的俯仰角和横滚角；

判断所述装载在机械臂上陀螺仪静止时刻的所述俯仰角和所述横滚角数值是否在该路段的预设角度区域；

若所述陀螺仪的仰角和滚角不在所述预设角度区域，则微调控制机械臂使陀螺仪的所述俯仰角和所述横滚角在所述预设角度区域以获取路段两端静止时刻的陀螺仪的静止姿态数据。

3.根据权利要求1所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述获取路段两端静止时刻的陀螺仪的静止姿态数据还包括：

当机械臂为有编码器反馈的机械臂时，获取预设航向；

控制所述机械臂将装载在机械臂上陀螺仪与所述预设航向对齐后获取陀螺仪静止姿态数据的航向角；

通过加速度计输出值计算出陀螺仪静止姿态角的俯仰角和横滚角，将所述俯仰角、所述横滚角和所述航向角作为所述静止姿态数据。

4.根据权利要求1所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述基于所述运动路径，控制机械臂沿所述运动路段运动并获取路段两端静止时刻的陀螺仪的静止姿态数据和路段中间的陀螺仪运动数据作为标定数据包括：

获取机械臂的第i静止姿态；

基于所述运动路径，获取静止姿态数量j；

在静止时间后，控制机械臂沿所述运动路径运动至第i+1静止姿态；

获取所述第i静止姿态到所述第i+1静止姿之间的所有的所述陀螺仪运动数据以及所述静止姿态数据作为所述标定数据，其中，1≤i≤j-1。

5.根据权利要求1所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述基于标定数据及陀螺仪补偿公式，获取最优补偿参数包括：

基于所述静止姿态数据，获取静止姿态向量；

基于所述静止姿态向量，构建每运动路段的初始姿态向量以及终止姿态向量；

基于所述初始姿态向量，获取初始姿态矩阵；

基于第i静止姿态到第i+1静止姿之间的所有的所述陀螺仪运动数据，以所述第i静止姿态为参考起点基于预设算法，获取由陀螺仪运动数据计算的终止姿态矩阵；

基于所述终止姿态矩阵及所述预设算法，获取陀螺仪运动数据计算的终止姿态向量；

基于加速度计算的终止姿态向量及陀螺仪运动数据计算的运动终止姿态向量，获取第i路段的姿态角差值；

判断所述姿态角差值是否满足角度差值要求；

若所述姿态角差值满足所述角度差值要求，则基于标定数据及陀螺仪补偿公式，获取最优补偿参数。

6.根据权利要求5所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述基于标定数据及陀螺仪补偿公式，获取最优补偿参数包括：

基于所述补偿参数、所述陀螺仪数据以及姿态误差公式，计算所述运动路径包含的全部路段的姿态角误差和；

获取最小的所述姿态角误差和；

基于最小的所述姿态角，获取对应的补偿参数作为所述最优补偿参数。

7.根据权利要求6所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述获取最小的所述姿态角误差和包括：

基于所述陀螺基础补偿公式，获取补偿参数；

获取所述补偿参数的数值范围、数值间隔以及抽样点数；

基于所述补偿参数的数值范围、数值间隔以及抽样点数，获取所有所述补偿参数集合；

基于所述补偿参数集合以及所述姿态误差公式，获取最小的所述姿态角误差和。

8.根据权利要求7所述的一种用机械臂标定陀螺仪的方法，其特征在于，所述获取最小的所述姿态角误差和之后包括：

判断所述最小的所述姿态角误差和是否满足预设要求；

若所述最小的所述姿态角误差和不满足所述预设要求，则修改所述数值范围以及所述数值间隔直至最小的所述姿态角误差和满足所述预设要求。

9.一种机械臂，应用于如权利要求1至权利要求8中任意一项的陀螺仪标定方法，通过机械臂获取所述陀螺仪运动数据以及所述静止姿态数据。