CN108225271A - 一种机器人倾倒检测的方法、系统、存储介质和设备 - Google Patents

Abstract

本发明提供机器人倾倒检测的方法、系统、存储介质和设备。本发明涉及机器人检测技术领域，尤其涉及一种机器人倾倒检测的方法、系统、存储介质和设备。一种机器人倾倒检测的方法，包括：连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻估测角度X(k)，将加速度仪测量数据作为k时刻观测角度Y(k)，k是大于等于1的整数；根据卡尔曼滤波原理，将估测角度和观测角度进行融合，获取机器人与地面的夹角；若机器人与地面的夹角超出预设范围，判断机器人倾倒。本发明一方面提供一种有效机器人倾倒检测方法；另一方面提供智能响应避免机器人倾倒，降低安全风险。

Description

一种机器人倾倒检测的方法、系统、存储介质和设备

技术领域

本发明涉及机器人检测技术领域，特别涉及一种机器人倾倒检测的方法、系统、存储介质和设备。

背景技术

机器人是自动执行工作的机器装置，一般由执行机构、驱动装置、检测装置和控制系统和复杂机械等组成。检测方法是实时检测机器人的运动及工作情况，根据需要反馈给控制系统，与设定信息进行比较后，对执行机构进行调整，以保证机器人的动作符合预定的要求。

目前已有一些检测机器人摔倒的方法，机器人装上各种壁障传感器，如红外测距传感器，激光测距传感器，摄像头，超声波传感器，碰撞开关等等。但上述每种机器人上的测距传感器只能测量有限的范围，且都有相应测量的盲区，因此在机器人运动过程中，难免会跟周围物体或人发生碰撞，为了避免在碰撞发生后中，机器人对自身，周围物体或人造成进一步的伤害。

综上所述，上述机器人倾倒检测的现有技术均适用范围有限、精确度不高。

发明内容

本发明为了解决上述现有机器人倾倒检测技术适用范围有限、精确度不高的问题，提供一种机器人倾倒检测的方法和防倾倒机器人，能够更有效、更精确的检测出机器人是否倾倒，从而提供自动控制以便降低安全风险。

为了实现本发明以上发明目的，本发明是通过以下技术方案实现的：

一种机器人倾倒检测的方法，其特征在于，包括：

步骤S100连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；

步骤S200根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；

步骤S300若所述k时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

上述技术方案中，机器人在运动时，在空间中有着特定的角度范围内，机器人可以正常运动；当大等于最大自平衡倾角时，物体则会倾倒。本实施例中默认一预设角度范围，则认为当机器人与地面夹角超出范围，将控制机器人智能响应。连续获取陀螺仪和加速度仪的测量数据，根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻机器人与地面的夹角进行融合，获取机器人与地面的夹角；由于同时融合了陀螺仪与加速度数据，再通过卡尔曼滤波算法来减少外部的干扰，从而能获得较为精确的夹角，提高了倾倒检测的精确度。

优选地，在所述步骤S200根据卡尔曼滤波原理，将所述估测角度和所述观测角度进行融合，获取所述机器人与地面的夹角具体包括：

步骤S210获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

步骤S220根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；

步骤S230根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

步骤S240通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

步骤S250通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k时刻机器人与地面的夹角；

步骤S260通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

上述技术方案中，根据角度微分等于时间的微分乘以角速度，陀螺仪k时刻角度可近似认为是k-1时刻的角度值加上k-1时刻测得的角加速度值乘以时间，即角度估测值＝上一次的最优角度+(角速度-陀螺仪的静态漂移)*dt。再根据X(k|k-1)＝AX(k-1|k-1)+BU(k)，计算k时刻估测角度X(k|k-1)。获取所述k时刻机器人与地面夹角，是一个递归的过程。根据估测方程，通过k-1时刻估测角度X(k-1|k-1)的协方差P(k-1|k-1)估测X(k|k-1)的协方差P(k|k-1)；计算卡尔曼增益Kg(k)＝P(k|k-1)H'/(HP(k|k-1)H'+R)，Kg是一个二维向量，对应陀螺仪角度和漂移的卡尔曼增益，R为加速度计测量出角度值的噪声，H＝|1 0|。结合加速度计测量的的角度值和加速度计算测量出角度值噪声值，并计算k时刻卡尔曼增益Kg(k)。获取最优的所述k时刻估测角度，更新k时刻估测角度的协方差。所述k时刻机器人与地面夹角为获取最优的所述k时刻估测角度。

优选地，在所述步骤S210获取卡尔曼算法中的参数，所述参数包括：系统的状态矩阵A、控制矩阵B和测量矩阵H、单位矩阵I，具体包括：

获取所述状态矩阵所述控制矩阵所述测量矩阵H＝|1 0|；I为单位矩阵

在上述技术方案中，根据角度微分等于时间的微分乘以角速度，陀螺仪k时刻角度可近似认为是k-1时刻的角度值加上k-1时刻测得的角加速度值乘以时间，即角度估测值＝上一次的最优角度+(角速度-陀螺仪的静态漂移)*dt。

对照卡尔曼算法的方程X(k|k-1)＝AX(k-1|k-1)+BU(k)，所述状态矩阵 所述控制矩阵所述测量矩阵H＝|1 0|，I为单位矩阵为计算k时刻机器人与地面夹角提供必要参数。

优选地，步骤S300之后进一步包括：当所述机器人倾倒时，控制所述机器人智能响应，所述响应包括语音提示用户停止运动或关闭系统电源。

在上述技术方案中，当所述机器人倾倒时，控制所述机器人智能响应，具体包括：语音提示用户停止运动或关闭系统电源。当检测到机器人倾倒，避免机器人倾倒后带来安全风险，机器人智能进行响应关闭系统电源终止机器人继续运动，或进行语音提示用户。

本发明提供一种基于机器人倾倒的检测系统，应用于机器人进行倾倒检测，所述基于机器人倾倒的检测系统包括：

角度采集模块，用于采集陀螺仪测量数据和加速度仪测量数据，并将所述采集数据发送至角度计算模块；

角度计算模块，与所述角度采集模块连接，用于将所述陀螺仪测量数据作为k时刻控制量，将所述加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数，以及根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，计算k时刻机器人与地面的夹角；

倾倒判断模块，与所述角度计算模块连接，用于当所述机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

本方案中的机器人倾倒检测系统至少包括：角度采集模块、角度计算模块和倾倒判断模块。本系统通过角度采集模块获取角度控制量和观测值；通过计算模块将角度估测值和观测值转换为与地面夹角，再根据卡尔曼过滤原理进行融合估测角度和观测角度，获得机器人与地面夹角；当机器人和地面夹角达到或超过预设值时，倾倒判断模块判断机器人倾倒。

优选地，所述角度计算模块，包括：

获取子模块，用于获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

角度估算子模块，与所述获取子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；

协方差计算子模块，与所述获取子模块连接和所述角度估算子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

卡尔曼增益计算子模块，与所述获取子模块和所述协方差计算子模块连接，用于通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

最优角度计算子模块，与所述获取子模块、所述角度估算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k时刻机器人与地面的夹角；

更新协方差子模块，与所述获取子模块、所述协方差计算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

进一步可选的，所述机器人倾倒检测系统还包括：倾倒处理模块，用于当所述机器人倾倒时，控制所述机器人智能响应，所述响应包括语音提示用户停止运动或关闭系统电源。

本发明提供一种存储介质，所述存储介质存储有多条指令，所述多条指令被一个或者多个处理器执行，以实现权利要求1-4中任一项所述的基于机器人倾倒检测方法的步骤。

本方案中的存储介质可用于存储软件程序及模块，比如本发明的机器人倾倒检测方法和系统对应的程序指令/模块。这些程序指令/模块可被处理器执行，实现上述的机器人倾倒检测方法和系统。

本发明提供一种防倾倒功能机器人设备，包括：

存储器；

一个或多个处理器；以及

一个或多个模块，所述一个或多个模块被存储在所述存储器中并配置成由所述一个或多个处理器执行，所述一个或多个模块包括用与执行以下步骤：

连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；

根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；

若所述k时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

上述技术方案中，本发明还提供了一种防倾倒机器人，具有与上述机器人防倾倒的功能，在检测倾倒部分有相同的技术效果，采用连续获取陀螺仪和加速度仪的测量数据，根据卡尔曼滤波原理，将估测角度和观测角度进行融合，获取机器人与地面的夹角。基于上述信息，判断机器人是否倾倒，此时控制机器人智能响应。本技术方案一方面能够提供一种有效机器人倾倒检测方法；另一方面提供智能响应避免机器人倾倒，降低安全风险。

与现有技术相比，本发明的有益效果在于：本发明提出一种机器人倾倒的检测的方法，充分利用了陀螺仪测量数据和加速度仪测量数据及卡尔曼滤波算法融合两种传感器测量数据得到的机器人与地面夹角，使得测量机器人与地面夹角的测量精度大大提高。在进一步的方案中，本发明采用计算估测角度方差的方法来计算卡尔曼滤波算法中的参数，提高计算机器人与地面夹角精度。

本发明至少具有以下有益效果之一：

1.一种有效机器人倾倒检测方法；

2.提供智能响应避免机器人倾倒，降低安全风险。

附图说明

附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：

图1为本发明机器人倾倒的检测的方法一实施例的流程示意图；

图2为本发明获取机器人与地面的夹角另一实施例的流程示意图；

图3为本发明机器人倾倒的检测的系统一实施例结构示意图；

图4为本发明机器人倾倒的检测的系统另一实施例结构示意图；

附图标号说明：

100.角度采集模块，200.角度计算模块，210.获取子模块，220.角度估算子模块，230.协方差计算子模块，240.卡尔曼增益计算子模块，250.最优角度计算子模块，260.更新协方差子模块，300.倾倒判断模块。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。一种基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

通常机器人地检测方法是实时检测机器人的运动及工作情况，根据需要反馈给控制系统，与设定信息进行比较后，对执行机构进行调整，以保证机器人的动作符合预定的要求。目前已有一些检测机器人摔倒的方法，如在机器人装上红外测距传感器，激光测距传感器等。但测距传感器只能测量有限的范围，且都有相应测量的盲区，因此在机器人运动过程中，难免会跟周围物体或人发生碰撞而倾倒。为了避免倾倒后对周围物体或人造成进一步的伤害，而本发明对此提供了解决方案。

参考图1所示，为本发明机器人倾倒的检测的方法一实施例的流程示意图，一种机器人倾倒检测的方法具体包括：

步骤S100连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；

步骤S200根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；

步骤S300若k时刻机器人与地面的夹角超出预设范围，判断机器人倾倒。

本实施例中的判断原理为：机器人在运动时，在空间中有着特定的角度范围内(在x、y、z三维坐标系中的角度)，机器人可以正常运动；当机器人与地面夹角大于等于最大自平衡倾角时，机器人则会倾倒。本实施例中，计算机器人与地面夹角需要陀螺仪测量数据和加速度仪测量数据，根据卡尔曼过滤原理将将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合得到。当机器人与地面夹角超出预设范围，控制机器人智能响应。需要说明的是，预设值与最大自平衡倾角可以相同，也可以不同。

卡尔曼滤波是以最小均方误差作为估计的最佳准则，获取机器人与地面夹角是一个递推的过程，参见图2，示出了为本发明机器人倾倒的检测的方法另一实施例的流程示意图。在步骤S200根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角，包括：

步骤S210获取卡尔曼算法中的参数，参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

步骤S220根据k-1时刻机器人与地面的夹角X(k-1|k-1)、k时刻控制量以及状态矩阵A、控制矩阵B，计算k时刻估测角度X(k|k-1)；

步骤S230根据k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、状态矩阵A，计算k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

步骤S240通过k时刻估测角度X(k|k-1)的协方差P(k|k-1)和测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

步骤S250通过k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、k时刻卡尔曼增益Kg(k)和测量矩阵H，获取k时刻最优估测角度X(k|k)，X(k|k)为k时刻机器人与地面的夹角；

步骤S260通过k-1时刻估测角度的协方差P(k|k-1)、k时刻卡尔曼增益Kg(k)和测量矩阵H、单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

具体地，相对上一个实施例，本实施例增加了步骤S210-步骤S260，在增加的步骤中，对获取的机器人的倾斜角速度进行积分，得到初始倾斜角度；对获取的机器人的倾斜加速度进行积分，得到换算角度；将初始倾斜角度和换算角度进行比较，得到误差角度；按照预设比例对误差角度进行放大，得到修正角度；将修正角度与初始倾斜角度进行叠加计算，得到倾斜角度。

根据卡尔曼滤波原理，建立如下五个方程：

估测角度时间状态转移方程：

X(k|k-1)＝A X(k-1|k-1)+B U(k) (1)

P(k|k-1)＝A P(k-1|k-1)A’+Q (2)

观测角度更新方程：

Kg(k)＝P(k|k-1)H’/(HP(k|k-1)H’+R) (3)

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)) (4)

P(k|k)＝(I-Kg(k)H)P(k|k-1) (5)

获取k时刻机器人与地面夹角为X(k|k)计算得到。

具体地，根据角度微分等于时间的微分乘以角速度，对应卡尔曼滤波第一个方程具体如式(1)， 估测方差阵的预测值，噪声就是数据的方差值。P(k|k-1)＝AP(k-1|k-1)A'+Q，P(k|k-1)是X(k|k-1)的协方差，P(k-1|k-1)是X(k-1|k-1)的协方差，Q是估计过程的误差协方差。公式中Q为向量A的协方差矩阵因为陀螺仪漂移噪声和角度噪声是相互独立的，所以cov(Q_Angleacc,Q_gyro)＝0，cov(Q_gyro,Q_Angleacc)＝0，所以设置角度噪声Q_Angleacc＝0.001，漂移噪声Q_gyro＝0.0005；对应卡尔曼滤波第二个方程具体如式(2)。计算卡尔曼增益K_0，K_1，结合设置角度测量噪声值为R_angle＝0.05；对应卡尔曼滤波第三个方程具体如式(3)。对矩阵P(k|k)进行更新对应卡尔曼滤波第五个方程具体如式(5)P(k|k)＝(I-Kg(k)H)P(k|k-1)。通过卡尔曼增益修正当前的角度、陀螺仪零点，算出当前角速度，对应卡尔曼滤波第四个方程具体如式(4)，X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))。其中，Z(k)是k时刻观测角度，H为测量矩阵。已知k-1时刻估测角度X(k|k-1)和k时刻观测角度Z(k)以及k时刻卡尔曼增益Kg(k)和测量矩阵H，获取k时刻最优估测角度X(k|k)，即为k时刻机器人与地面的夹角。计算机器人与地面夹角是进行一个递归过程，随着时间变化，计算估测角度和估测角度状态量的协方差，需要更新k时刻估测角度的协方差，P(k|k)＝(I-Kg(k)H)P(k|k-1)。

参考图3所示为本发明机器人倾倒的检测的一实施例系统结构示意图。本发明提供一种基于机器人倾倒的检测系统，应用于机器人进行倾倒检测，该系统包括：角度采集模块100，用于采集陀螺仪测量数据和加速度仪测量数据，并将采集数据发送到角度计算模块；

角度计算模块200，与角度采集模块连接，用于将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数，以及根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，计算k时刻机器人与地面的夹角；

倾倒判断模块300，与角度计算模块连接，用于当机器人与地面的夹角超出预设范围，判断机器人倾倒。

具体地，机器人倾倒的检测的系统通过角度采集模块100接收陀螺仪测量数据和加速度仪测量数据，并将采集数据发送到角度计算模块200；角度计算模块200，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，计算k时刻机器人与地面的夹角；倾倒判断模块300，从角度计算模块200收到根据机器人与地面的当前夹角，根据预设角度范围，判断机器人是否倾倒。

参考图4所示为本发明机器人倾倒的检测的另一实施例系统结构示意图。角度计算模块200，包括：获取子模块210、角度估算子模块220、协方差计算子模块230、卡尔曼增益计算子模块240、最优角度计算子模块250和更新协方差子模块260；

获取子模块210，用于获取卡尔曼算法中的参数，参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

角度估算子模块220，与获取子模块210连接，用于根据k-1时刻机器人与地面的夹角X(k-1|k-1)、k时刻控制量以及状态矩阵A、控制矩阵B，计算k时刻估测角度X(k|k-1)；

协方差计算子模块230，与获取子模块210和角度估算子模块220连接，用于根据k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、状态矩阵A，计算k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

卡尔曼增益计算子模块240，与获取子模块210和协方差计算子模块230连接，用于通过k时刻估测角度X(k|k-1)的协方差P(k|k-1)和测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

最优角度计算子模块250，与获取子模块210、角度估算子模块220和卡尔曼增益计算子模块240连接，用于通过k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、k时刻卡尔曼增益Kg(k)和测量矩阵H，获取k时刻最优估测角度X(k|k)，X(k|k)为k时刻机器人与地面的夹角；

更新协方差子模块260，与获取子模块210、协方差计算子模块230和卡尔曼增益计算子模块240连接，用于通过k-1时刻估测角度的协方差P(k|k-1)、k时刻卡尔曼增益Kg(k)和测量矩阵H、单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

具体地，与上一实施例相比，本实施例将角度计算模块替换成：获取子模块210、角度估算子模块220、协方差计算子模块230、卡尔曼增益计算子模块240、最优角度计算子模块250和更新协方差子模块260；获取子模块210接收角度计算模块200采集数据，获取获取卡尔曼算法中的状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；角度估计算子模块220计算k时刻估测角度X(k|k-1)、协方差计算子模块230计算k时刻估测角度X(k|k-1)的协方差P(k|k-1)；卡尔曼增益计算子模块240通过k时刻估测角度X(k|k-1)的协方差P(k|k-1)和测量矩阵H，计算k时刻卡尔曼增益Kg(k)；最优角度计算子模块260用于计算出k时刻机器人与地面的夹角；以及更新协方差子模块260通过k-1时刻估测角度的协方差P(k|k-1)、k时刻卡尔曼增益Kg(k)和测量矩阵H、单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

可选的，该系统还可以包括倾倒处理模块400，用于当所述机器人倾倒时，控制所述机器人智能响应，所述响应包括语音提示用户停止运动或关闭系统电源。

本发明的系统实施例可集成在机器人内，使得准确获取机器人倾倒的角度，判断是否发生倾倒，并进行智能控制。可以理解，图3所示的结构仅仅为示意，并不是对机器人的结构造成限定，机器人还可以包括比图3所示更多或者更少的组件，或者具有与图3所示不同的机器人。图3所示的各组件可以采用硬件、软件或其组合实现。

本发明还公开了一种存储介质，存储介质存储有多条指令，多条指令被一个或者多个处理器执行，以实现下述步骤：连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据获取k时刻的控制量，将加速度仪测量数据作为k时刻的观测角度，k是大于等于1的整数；根据卡尔曼滤波原理，将k时刻的控制量和k时刻的观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；若机器人与地面的夹角超出预设范围，判断机器人倾倒。

本存储介质方案中，将估测角度和观测角度进行融合，获取机器人与地面的夹角的具体的方程建立、参数和计算方法等与上述的机器人倾倒检测方法的实施例相同。本方案中的存储介质可用于存储软件程序及模块，比如本发明的机器人倾倒检测方法和系统对应的程序指令/模块。这些程序指令/模块可被处理器执行，实现上述的机器人倾倒检测方法和系统。

较佳的，在上述基础上，本发明存储介质的另一实施例，存储介质存储有多条指令，多条指令被一个或者多个处理器执行，以实现任一本发明机器人倾倒检测方法实施例的步骤。

本发明还提供一种防倾倒功能机器人设备，包括：

存储器；一个或多个处理器；以及一个或多个模块，一个或多个模块被存储在存储器中并配置成由一个或多个处理器执行，一个或多个模块包括用与执行以下步骤：连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；若k时刻机器人与地面的夹角超出预设范围，判断机器人倾倒。

本设备方案中，将估测角度和观测角度进行融合，获取机器人与地面的夹角的具体的方程建立、参数和计算方法等与上述的机器人倾倒检测方法的实施例相同。

其中，在上述基础上，本发明机器人的一个实施例，具有上述机器人防倾倒的功能，在检测倾倒部分有相同的技术效果，采用实时获取陀螺仪和加速度仪的测量数据，建立角度估测状态方程和角度观测方程。根据卡尔曼滤波原理，将估测角度和观测角度进行融合，获取机器人与地面的夹角。基于上述信息，判断机器人是否倾倒，此时控制机器人智能响应。

本发明实施例中，机器人倾倒检测的方法及系统、存储介质与机器人属于同一构思，在机器人上，通过处理器执行存储介质中存储的指令，可以运行相应的机器人倾倒检测的方法实施例中提供的方法，其具体实现过程详见前面的机器人倾倒检测的方法实施例，此处不再赘述。

需要说明的是，本领域普通测试人员可以理解实现本发明实施例机器人倾倒检测的方法的全部或部分流程，是可以通过计算机程序来控制相关的硬件来完成，计算机程序可存储于计算机可读取存储介质中，如存储在机器人的存储器中，并被该机器人内的至少一个处理器执行，在执行过程中可包括如机器人倾倒检测的方法实施例的流程。

在本发明中，如果以软件功能单元的形式实现并作为独立的产品销售或者使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术的贡献的部分或者该技术方案的全部或者部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可是个人计算机，服务器，或者网络设备)，或者处理器执行本申请个实施例方法的全部或部分步骤。而前述的存储介质包数据服务器，云端服务器，只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、移动通信设备，或者光盘、或者U盘等各种可以存储代码的介质。

此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。

Claims (9)

1.一种机器人倾倒检测的方法，其特征在于，包括：

步骤S100连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；

步骤S200根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；

步骤S300若所述k时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

2.根据权利要求1所述的机器人倾倒检测方法，其特征在于，在所述步骤S200根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角，包括：

步骤S210获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

步骤S220根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；

步骤S230根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

步骤S240通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

步骤S250通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k时刻机器人与地面的夹角；

步骤S260通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

3.根据权利要求4所述的机器人倾倒检测方法，其特征在于，所述步骤S210获取卡尔曼算法中的参数，所述参数包括：系统的状态矩阵A、控制矩阵B和测量矩阵H、单位矩阵I，具体包括：

获取所述状态矩阵所述控制矩阵所述测量矩阵H＝|1 0|；

I为单位矩阵

4.根据权利要求1所述的机器人倾倒检测方法，其特征在于，步骤S300之后进一步包括：当所述机器人倾倒时，控制所述机器人智能响应，所述响应包括语音提示用户停止运动或关闭系统电源。

5.一种基于机器人倾倒的检测系统，其特征在于，应用于机器人进行倾倒检测，所述基于机器人倾倒的检测系统包括：

角度采集模块，用于采集陀螺仪测量数据和加速度仪测量数据，并将所述采集数据发送至角度计算模块；

角度计算模块，与所述角度采集模块连接，用于将所述陀螺仪测量数据作为k时刻控制量，将所述加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数，以及根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，计算k时刻机器人与地面的夹角；

倾倒判断模块，与所述角度计算模块连接，用于当所述机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

6.根据权利要求5所述的机器人倾倒检测系统，其特征在于，所述角度计算模块，包括：

获取子模块，用于获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；

角度估算子模块，与所述获取子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；

协方差计算子模块，与所述获取子模块连接和所述角度估算子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；

卡尔曼增益计算子模块，与所述获取子模块和所述协方差计算子模块连接，用于通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；

最优角度计算子模块，与所述获取子模块、所述角度估算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k时刻机器人与地面的夹角；

更新协方差子模块，与所述获取子模块、所述协方差计算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

7.根据权利要求5所述的机器人倾倒检测系统，其特征在于，进一步还包括：倾倒处理模块，用于当所述机器人倾倒时，控制所述机器人智能响应，所述响应包括语音提示用户停止运动或关闭系统电源。

8.一种存储介质，其特征在于，所述存储介质存储有多条指令，所述多条指令被一个或者多个处理器执行，以实现权利要求1-4中任一项所述的基于机器人倾倒检测方法的步骤。

9.一种防倾倒功能机器人设备，包括：

存储器；

一个或多个处理器；以及

一个或多个模块，所述一个或多个模块被存储在所述存储器中并配置成由所述一个或多个处理器执行，所述一个或多个模块包括用与执行以下步骤：

连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据获取k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；

根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；

若所述k时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。