一种机器人倾倒检测的方法、系统、存储介质和设备[发明专利]

(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 201810067863.9
(22)申请日 2018.01.24
(71)申请人 南京阿凡达机器人科技有限公司
地址 211316 江苏省南京市高淳经济开发
区古檀大道3号
(72)发明人 李庭亮　
(74)专利代理机构 上海硕力知识产权代理事务
所(普通合伙) 31251
代理人 郭桂峰
(51)Int.Cl.
G01C 9/00(2006.01)
(54)发明名称
一种机器人倾倒检测的方法、系统、存储介
质和设备
(57)摘要
本发明提供机器人倾倒检测的方法、系统、
存储介质和设备。

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

一种机器人倾倒检测的方法，
包括：连续获取陀螺仪测量数据和加速度仪测量
数据，将陀螺仪测量数据作为k时刻估测角度X
(k )，将加速度仪测量数据作为k时刻观测角度Y
(k )，k是大于等于1的整数；根据卡尔曼滤波原
理，将估测角度和观测角度进行融合，获取机器
人与地面的夹角；若机器人与地面的夹角超出预
设范围，判断机器人倾倒。

本发明一方面提供一
种有效机器人倾倒检测方法；另一方面提供智能
响应避免机器人倾倒，
降低安全风险。

权利要求书2页 说明书9页 附图3页CN 108225271 A 2018.06.29
C N 108225271
A
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时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

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

背景技术
[0002]机器人是自动执行工作的机器装置，一般由执行机构、驱动装置、检测装置和控制系统和复杂机械等组成。

检测方法是实时检测机器人的运动及工作情况，根据需要反馈给控制系统，与设定信息进行比较后，对执行机构进行调整，以保证机器人的动作符合预定的要求。

[0003]目前已有一些检测机器人摔倒的方法，机器人装上各种壁障传感器，如红外测距传感器，激光测距传感器，摄像头，超声波传感器，碰撞开关等等。

但上述每种机器人上的测距传感器只能测量有限的范围，且都有相应测量的盲区，因此在机器人运动过程中，难免会跟周围物体或人发生碰撞，为了避免在碰撞发生后中，机器人对自身，周围物体或人造成进一步的伤害。

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

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

[0006]为了实现本发明以上发明目的，本发明是通过以下技术方案实现的：
[0007]一种机器人倾倒检测的方法，其特征在于，包括：
[0008]步骤S100连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；
[0009]步骤S200根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；
[0010]步骤S300若所述k时刻机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

[0011]上述技术方案中，机器人在运动时，在空间中有着特定的角度范围内，机器人可以正常运动；当大等于最大自平衡倾角时，物体则会倾倒。

本实施例中默认一预设角度范围，则认为当机器人与地面夹角超出范围，将控制机器人智能响应。

连续获取陀螺仪和加速度仪的测量数据，根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻机器人与地面的夹角进行融合，获取机器人与地面的夹角；由于同时融合了陀螺仪与加速度数据，再通过卡尔曼滤波算法来减少外部的干扰，从而能获得较为精确的夹角，提高了倾倒检测的精确度。

[0012]优选地，在所述步骤S200根据卡尔曼滤波原理，将所述估测角度和所述观测角度进行融合，获取所述机器人与地面的夹角具体包括：
[0013]步骤S210获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；
[0014]步骤S220根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；
[0015]步骤S230根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；
[0016]步骤S240通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；
[0017]步骤S250通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k时刻机器人与地面的夹角；
[0018]步骤S260通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg (k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

[0019]上述技术方案中，根据角度微分等于时间的微分乘以角速度，陀螺仪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时刻估测角度。

[0020]优选地，在所述步骤S210获取卡尔曼算法中的参数，所述参数包括：系统的状态矩阵A、控制矩阵B和测量矩阵H、单位矩阵I，具体包括：
[0021]获取所述状态矩阵所述控制矩阵所述测量矩阵H＝|1 0|；I
为单位矩阵
[0022]在上述技术方案中，根据角度微分等于时间的微分乘以角速度，陀螺仪k时刻角度可近似认为是k-1时刻的角度值加上k-1时刻测得的角加速度值乘以时间，即角度估测值＝上一次的最优角度+(角速度-陀螺仪的静态漂移)*dt。

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

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

[0026]在上述技术方案中，当所述机器人倾倒时，控制所述机器人智能响应，具体包括：语音提示用户停止运动或关闭系统电源。

当检测到机器人倾倒，避免机器人倾倒后带来安全风险，机器人智能进行响应关闭系统电源终止机器人继续运动，或进行语音提示用户。

[0027]本发明提供一种基于机器人倾倒的检测系统，应用于机器人进行倾倒检测，所述基于机器人倾倒的检测系统包括：
[0028]角度采集模块，用于采集陀螺仪测量数据和加速度仪测量数据，并将所述采集数据发送至角度计算模块；
[0029]角度计算模块，与所述角度采集模块连接，用于将所述陀螺仪测量数据作为k时刻控制量，将所述加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数，以及根据卡尔曼滤波原理，将所述k时刻控制量和所述k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，计算k时刻机器人与地面的夹角；
[0030]倾倒判断模块，与所述角度计算模块连接，用于当所述机器人与地面的夹角超出预设范围，判断所述机器人倾倒。

[0031]本方案中的机器人倾倒检测系统至少包括：角度采集模块、角度计算模块和倾倒判断模块。

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

[0032]优选地，所述角度计算模块，包括：
[0033]获取子模块，用于获取卡尔曼算法中的参数，所述参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；
[0034]角度估算子模块，与所述获取子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)、所述k时刻控制量以及所述状态矩阵A、所述控制矩阵B，计算k时刻估测角度X(k|k-1)；
[0035]协方差计算子模块，与所述获取子模块连接和所述角度估算子模块连接，用于根据所述k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、所述状态矩阵A，计算所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)；
[0036]卡尔曼增益计算子模块，与所述获取子模块和所述协方差计算子模块连接，用于通过所述k时刻估测角度X(k|k-1)的协方差P(k|k-1)和所述测量矩阵H，获取k时刻卡尔曼增益Kg(k)；
[0037]最优角度计算子模块，与所述获取子模块、所述角度估算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、所述k 时刻卡尔曼增益Kg(k)和所述测量矩阵H，获取k时刻最优估测角度X(k|k)，所述X(k|k)为k
时刻机器人与地面的夹角；
[0038]更新协方差子模块，与所述获取子模块、所述协方差计算子模块和所述卡尔曼增益计算子模块连接，用于通过所述k-1时刻估测角度的协方差P(k|k-1)、所述k时刻卡尔曼增益Kg(k)和所述测量矩阵H、所述单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

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

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

[0041]本方案中的存储介质可用于存储软件程序及模块，比如本发明的机器人倾倒检测方法和系统对应的程序指令/模块。

这些程序指令/模块可被处理器执行，实现上述的机器人倾倒检测方法和系统。

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

[0049]上述技术方案中，本发明还提供了一种防倾倒机器人，具有与上述机器人防倾倒的功能，在检测倾倒部分有相同的技术效果，采用连续获取陀螺仪和加速度仪的测量数据，根据卡尔曼滤波原理，将估测角度和观测角度进行融合，获取机器人与地面的夹角。

基于上述信息，判断机器人是否倾倒，此时控制机器人智能响应。

本技术方案一方面能够提供一种有效机器人倾倒检测方法；另一方面提供智能响应避免机器人倾倒，降低安全风险。

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

在进一步的方案中，本发明采用计算估测角度方差的方法来计算卡尔曼滤波算法中的参数，提高计算机器人与地面夹角精度。

[0051]本发明至少具有以下有益效果之一：
[0052] 1.一种有效机器人倾倒检测方法；
[0053] 2.提供智能响应避免机器人倾倒，降低安全风险。

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

在附图中：
[0055]图1为本发明机器人倾倒的检测的方法一实施例的流程示意图；
[0056]图2为本发明获取机器人与地面的夹角另一实施例的流程示意图；
[0057]图3为本发明机器人倾倒的检测的系统一实施例结构示意图；
[0058]图4为本发明机器人倾倒的检测的系统另一实施例结构示意图；
[0059]附图标号说明：
[0060]100.角度采集模块，200.角度计算模块，210.获取子模块，220.角度估算子模块，230.协方差计算子模块，240.卡尔曼增益计算子模块，250.最优角度计算子模块，260.更新协方差子模块，300.倾倒判断模块。

具体实施方式
[0061]为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

一种基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

[0062]通常机器人地检测方法是实时检测机器人的运动及工作情况，根据需要反馈给控制系统，与设定信息进行比较后，对执行机构进行调整，以保证机器人的动作符合预定的要求。

目前已有一些检测机器人摔倒的方法，如在机器人装上红外测距传感器，激光测距传感器等。

但测距传感器只能测量有限的范围，且都有相应测量的盲区，因此在机器人运动过程中，难免会跟周围物体或人发生碰撞而倾倒。

为了避免倾倒后对周围物体或人造成进一步的伤害，而本发明对此提供了解决方案。

[0063]参考图1所示，为本发明机器人倾倒的检测的方法一实施例的流程示意图，一种机器人倾倒检测的方法具体包括：
[0064]步骤S100连续获取陀螺仪测量数据和加速度仪测量数据，将陀螺仪测量数据作为k时刻控制量，将加速度仪测量数据作为k时刻观测角度，k是大于等于1的整数；
[0065]步骤S200根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角；
[0066]步骤S300若k时刻机器人与地面的夹角超出预设范围，判断机器人倾倒。

[0067]本实施例中的判断原理为：机器人在运动时，在空间中有着特定的角度范围内(在x、y、z三维坐标系中的角度)，机器人可以正常运动；当机器人与地面夹角大于等于最大自平衡倾角时，机器人则会倾倒。

本实施例中，计算机器人与地面夹角需要陀螺仪测量数据和加速度仪测量数据，根据卡尔曼过滤原理将将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合得到。

当机器人与地面夹角超出预设范围，控制机器人智能响应。

需要说明的是，预设值与最大自平衡倾角可以相同，也可以不同。

[0068]卡尔曼滤波是以最小均方误差作为估计的最佳准则，获取机器人与地面夹角是一个递推的过程，参见图2，示出了为本发明机器人倾倒的检测的方法另一实施例的流程示意图。

在步骤S200根据卡尔曼滤波原理，将k时刻控制量和k时刻观测角度以及k-1时刻的机器人与地面的夹角进行融合，获取k时刻机器人与地面的夹角，包括：
[0069]步骤S210获取卡尔曼算法中的参数，参数包括：状态矩阵A、控制矩阵B和测量矩阵H，单位矩阵I；
[0070]步骤S220根据k-1时刻机器人与地面的夹角X(k-1|k-1)、k时刻控制量以及状态矩阵A、控制矩阵B，计算k时刻估测角度X(k|k-1)；
[0071]步骤S230根据k-1时刻机器人与地面的夹角X(k-1|k-1)的协方差P(k-1|k-1)、状态矩阵A，计算k时刻估测角度X(k|k-1)的协方差P(k|k-1)；
[0072]步骤S240通过k时刻估测角度X(k|k-1)的协方差P(k|k-1)和测量矩阵H，获取k时刻卡尔曼增益Kg(k)；
[0073]步骤S250通过k-1时刻估测角度X(k|k-1)、k时刻观测角度Z(k)、k时刻卡尔曼增益Kg(k)和测量矩阵H，获取k时刻最优估测角度X(k|k)，X(k|k)为k时刻机器人与地面的夹角；[0074]步骤S260通过k-1时刻估测角度的协方差P(k|k-1)、k时刻卡尔曼增益Kg(k)和测量矩阵H、单位矩阵I，更新k时刻最优估测角度的协方差P(k|k)。

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

[0076]根据卡尔曼滤波原理，建立如下五个方程：
[0077]估测角度时间状态转移方程：
[0078]X(k|k-1)＝A X(k-1|k-1)+B U(k) (1)
[0079]P(k|k-1)＝A P(k-1|k-1)A’+Q (2)
[0080]观测角度更新方程：
[0081]Kg(k)＝P(k|k-1)H’/(HP(k|k-1)H’+R) (3)
[0082]X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)) (4)
[0083]P(k|k)＝(I-Kg(k)H)P(k|k-1) (5)
[0084]获取k时刻机器人与地面夹角为X(k|k)计算得到。

[0085]具体地，根据角度微分等于时间的微分乘以角速度，对应卡尔曼滤波第一个方程
具体如式(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，。