机器人基于六轴陀螺仪检测仰角的方法和检测被卡的方法技术

本发明专利技术涉及一种机器人基于六轴陀螺仪检测仰角的方法和检测被卡的方法。其中，所述检测仰角的方法，通过将陀螺仪所检测到的竖直方向的加速度与重力加速度进行求方差，如果所求得的方差值在预设方差范围内，表明机器人在水平面上，将机器人的仰角角度校准为零，否则，表明机器人是在斜面上，将机器人的仰角角度确定为六轴陀螺仪检测到的绕X轴转动的角度值，如此可以避免陀螺仪的零点漂移或其它干扰所带来的检测误差，提高了机器人检测仰角的准确性。此外，所述检测被卡的方法，通过将第一预设时间段内所记录的仰角数据与机器人中预先存储的参考仰角数据进行比较，并结合时间和打滑情况的综合判断，准确得出机器人是否被卡的检测结果。

Method for detecting elevation angle based on six axis gyroscope and method for detecting stuck card

The invention relates to a method for detecting elevation angle of a robot based on a six axis gyroscope and a method for detecting the stuck. The method for detecting elevation angle is to calculate the variance between the acceleration in the vertical direction detected by the gyroscope and the acceleration in the gravity direction. If the variance is within the preset range of variance, it indicates that the robot calibrates the elevation angle of the robot to zero on the horizontal plane, otherwise, it indicates that the robot is on the inclined plane. On the other hand, the elevation angle of the robot is determined as the angle of rotation around the X axis detected by the six-axis gyroscope, so as to avoid the detection error caused by the zero drift of the gyroscope or other interference, and improve the accuracy of the robot elevation detection. In addition, by comparing the elevation data recorded in the first preset time period with the reference elevation data stored in advance in the robot, and combining the comprehensive judgment of time and skid condition, the method accurately obtains the detection result of whether the robot is stuck or not.

【技术实现步骤摘要】
机器人基于六轴陀螺仪检测仰角的方法和检测被卡的方法
本专利技术涉及机器人领域，具体涉及一种机器人基于六轴陀螺仪检测仰角的方法和检测被卡的方法。
技术介绍
六轴陀螺仪可以同时检测角速度和加速度，装配在扫地机器人上，常用于姿态的判断。一般将六轴陀螺仪检测到的角速度进行时间积分就可以得到对应的旋转角度。然而，由于各种原因，所述陀螺仪上往往被作用有各种干扰力矩，在这些很小干扰力矩的作用下，陀螺仪的陀螺会产生进动，从而使角动量慢慢偏离原来的方向，我们把这种现象称为零点漂移。陀螺仪存在的零点漂移现象会影响传感器的检测精度。在扫地机中，如果检测到的俯仰角发生偏移，机器人会因此而产生误判，比如，机器人是处于水平面上的，却可能会被判断为在斜面上。此外，在实际家庭环境中，大多数家庭都有立地电风扇、门槛和吧台座椅等等，有些办公地方还有一些凳脚是U型的座椅，该座椅接触地面的凳脚是U型的不锈钢管，管的横截面为扁圆形状。扫地机器人在进行清扫工作时，容易被立地电风扇底座、门槛、吧台座椅的底座或者U型凳脚卡住，导致机器人出现打滑或者被卡住无法移动的问题，从而影响了机器人构建地图的准确性，导航准确性也因此大大降低。目前，很多机器人都无法识别是否被立地电风扇底座、门槛、吧台座椅的底座或者U型凳脚卡住的问题，有的机器人在这方面可以做一些改善，但是需要额外增加视觉或者激光等传感器的检测，导致机器人的成本增加，不利于推广应用。
技术实现思路
为解决上述问题，本专利技术提供了一种机器人基于六轴陀螺仪检测仰角的方法和检测被卡的方法，可以准确地检测机器人的仰角角度，且在不需要增加硬件成本的基础上准确检测出机器人是否被卡。本专利技术的具体技术方案如下：一种机器人基于六轴陀螺仪检测仰角的方法，包括如下步骤：步骤S1：确定机器人的六轴陀螺仪中的竖直方向的加速度计在第一预定时间段所检测到的加速度，并进入步骤S2；步骤S2：判断所述加速度与重力加速度的方差值是否在预设方差范围内，如果是，则进入步骤S3，如果否，则进入步骤S4；步骤S3：确定机器人的仰角角度为零；步骤S4：确定六轴陀螺仪检测到的绕X轴转动的角度值为机器人的仰角角度；其中，机器人的左右方向为X轴，前后方向为Y轴，上下方向为Z轴，X轴、Y轴和Z轴构成所述六轴陀螺仪的三维坐标系。进一步地，所述步骤S1具体包括如下步骤：分析第一预定时间段内，机器人的六轴陀螺仪中的竖直方向的加速度计所检测到的加速度，如果当前分析的加速度与其前一个加速度的差值大于预设加速度值，且当前分析的加速度与其后一个加速度的差值也大于预设加速度值，则确定当前分析的加速度为干扰数据，并删除所述干扰数据，否则，保留当前分析的加速度，把所保留的加速度确定为机器人的六轴陀螺仪中的竖直方向的加速度计在第一预定时间段所检测到的加速度。进一步地，步骤S2中所述的判断所述加速度与重力加速度的方差值是否在预设方差范围内，具体包括如下步骤：确定所述加速度与所述重力加速度的差值为加速度差值；确定所述加速度差值的平方之和的平均值为加速度平均值；确定所述加速度平均值的算术平方根为方差值；基于系统配置的预设方差范围，确定预设方差最大值和预设方差最小值；将所述方差值分别与所述预设方差最大值和所述预设方差最小值进行比较，如果所述方差值大于所述预设方差最小值，且小于所述预设方差最大值，则确定所述加速度与重力加速度的方差值在预设方差范围内，否则确定所述加速度与重力加速度的方差值不在预设方差范围内。进一步地，所述步骤S4具体包括如下步骤：步骤S41：确定机器人的六轴陀螺仪检测到的绕X轴转动的角速度；步骤S42：将所述角速度进行时间积分得到机器人绕X轴转动的X角度；步骤S43：确定所述六轴陀螺仪的零点漂移值；步骤S44：确定所述机器人的仰角角度为所述X角度与所述零点漂移值的差值。进一步地，所述步骤S43具体包括如下步骤：当机器人处于静止状态时，把机器人的六轴陀螺仪检测到的绕X轴转动的角速度进行时间积分所得到的角度值作为所述六轴陀螺仪的零点漂移值。其中，所述静止状态是指机器人开机后没有移动前的状态或者机器人在移动过程中停止下来时的状态，且所述状态需要持续第二预定时间段。一种机器人基于六轴陀螺仪检测被卡的方法，包括如下步骤：步骤1：根据上述的机器人基于六轴陀螺仪检测仰角的方法，记录机器人检测到的仰角数据，并进入步骤2；步骤2：将第一预设时间段内所记录的仰角数据与机器人中预先存储的参考仰角数据进行比较，判断所记录的仰角数据是否在参考仰角数据的误差范围内，如果是，则进入步骤3，如果否，则将所述第一预设时间段内所记录的仰角数据中的第一个记录的数据删除，并返回步骤1；步骤3：持续记录机器人检测到的仰角数据，并判断所记录的仰角数据在参考仰角数据的误差范围内的持续时间是否达到第二预设时间段，如果是，则进入步骤4，如果否，则确定机器人没有被卡；步骤4：判断在所述第二预设时间段内是否检测到机器人打滑，如果检测到机器人打滑，则确定机器人被卡，如果没有检测到机器人打滑，则确定机器人没有被卡。进一步地，步骤1中所述的记录机器人检测到的仰角数据，包括如下步骤：每间隔第三预设时间段，记录一个仰角角度；基于仰角角度为Y轴，记录时间为X轴的坐标系中，确定所述机器人所记录的仰角角度所构成的连线。进一步地，步骤2中所述的将第一预设时间段内所记录的仰角数据与机器人中预先存储的参考仰角数据进行比较，判断所记录的仰角数据是否在参考仰角数据的误差范围内，包括如下步骤：步骤21：获取机器人当前所记录的第一预设时间段内的仰角角度所构成的连线，确定所述连线的变化情况，并进入步骤22；步骤22：获取机器人中预先存储的机器人行走至不同斜面时，在所述第一预设时间段内所检测到的参考仰角数据所构成的参考线，并进入步骤23；步骤23：判断当前记录的所述连线的变化情况是否在所述参考线的变化情况的误差范围内，如果是，则确定所记录的仰角数据在参考仰角数据的误差范围内，如果否，则确定所记录的仰角数据不在参考仰角数据的误差范围内。进一步地，步骤4中所述的判断在所述第二预设时间段内是否检测到机器人打滑，包括如下步骤：步骤41：基于机器人的两个驱动轮上的码盘检测到的数据，确定当前记录时间点所检测到的所述两个驱动轮中的第一驱动轮所行进的第一当前行进距离，和上一记录时间点所检测到的第一驱动轮所行进的第一上一行进距离，计算得出第一驱动轮所行进的第一距离为所述第一当前行进距离与所述第一上一行进距离的差值；确定所述当前记录时间点所检测到的所述两个驱动轮中的第二驱动轮所行进的第二当前行进距离，和所述上一记录时间点所检测到的第二驱动轮所行进的第二上一行进距离，计算得出第二驱动轮所行进的第二距离为所述第二当前行进距离与所述第二上一行进距离的差值；确定所述行进距离差值为所述第一距离与所述第二距离的差值；确定所述两个驱动轮在第一预设时间内的行进角度值为所述行进距离差值与两个驱动轮之间的宽度的比值；确定第一预设时间内两个驱动轮所产生的第一角度变化率为所述行进角度值与所述第一预设时间的比值，并进入步骤42；步骤42：确定所述驱动轮的最大误差率，确定所述第一角度变化率的最大误差值为所述第一角度变化率与所述最大误差率的乘积，并进入步骤43；步骤43：基于机器人的六轴陀螺仪检测到本文档来自技高网...

【技术保护点】
1.机器人基于六轴陀螺仪检测仰角的方法，其特征在于，包括如下步骤：步骤S1：确定机器人的六轴陀螺仪中的竖直方向的加速度计在第一预定时间段所检测到的加速度，并进入步骤S2；步骤S2：判断所述加速度与重力加速度的方差值是否在预设方差范围内，如果是，则进入步骤S3，如果否，则进入步骤S4；步骤S3：确定机器人的仰角角度为零；步骤S4：确定六轴陀螺仪检测到的绕X轴转动的角度值为机器人的仰角角度；其中，机器人的左右方向为X轴，前后方向为Y轴，上下方向为Z轴，X轴、Y轴和Z轴构成所述六轴陀螺仪的三维坐标系。

【技术特征摘要】
1.机器人基于六轴陀螺仪检测仰角的方法，其特征在于，包括如下步骤：步骤S1：确定机器人的六轴陀螺仪中的竖直方向的加速度计在第一预定时间段所检测到的加速度，并进入步骤S2；步骤S2：判断所述加速度与重力加速度的方差值是否在预设方差范围内，如果是，则进入步骤S3，如果否，则进入步骤S4；步骤S3：确定机器人的仰角角度为零；步骤S4：确定六轴陀螺仪检测到的绕X轴转动的角度值为机器人的仰角角度；其中，机器人的左右方向为X轴，前后方向为Y轴，上下方向为Z轴，X轴、Y轴和Z轴构成所述六轴陀螺仪的三维坐标系。2.根据权利要求1所述的方法，其特征在于，所述步骤S1具体包括如下步骤：分析第一预定时间段内，机器人的六轴陀螺仪中的竖直方向的加速度计所检测到的加速度，如果当前分析的加速度与其前一个加速度的差值大于预设加速度值，且当前分析的加速度与其后一个加速度的差值也大于预设加速度值，则确定当前分析的加速度为干扰数据，并删除所述干扰数据，否则，保留当前分析的加速度，把所保留的加速度确定为机器人的六轴陀螺仪中的竖直方向的加速度计在第一预定时间段所检测到的加速度。3.根据权利要求2所述的方法，其特征在于，步骤S2中所述的判断所述加速度与重力加速度的方差值是否在预设方差范围内，具体包括如下步骤：确定所述加速度与所述重力加速度的差值为加速度差值；确定所述加速度差值的平方之和的平均值为加速度平均值；确定所述加速度平均值的算术平方根为方差值；基于系统配置的预设方差范围，确定预设方差最大值和预设方差最小值；将所述方差值分别与所述预设方差最大值和所述预设方差最小值进行比较，如果所述方差值大于所述预设方差最小值，且小于所述预设方差最大值，则确定所述加速度与重力加速度的方差值在预设方差范围内，否则确定所述加速度与重力加速度的方差值不在预设方差范围内。4.根据权利要求3所述的方法，其特征在于，所述步骤S4具体包括如下步骤：步骤S41：确定机器人的六轴陀螺仪检测到的绕X轴转动的角速度；步骤S42：将所述角速度进行时间积分得到机器人绕X轴转动的X角度；步骤S43：确定所述六轴陀螺仪的零点漂移值；步骤S44：确定所述机器人的仰角角度为所述X角度与所述零点漂移值的差值。5.根据权利要求4所述的方法，其特征在于，所述步骤S43具体包括如下步骤：当机器人处于静止状态时，把机器人的六轴陀螺仪检测到的绕X轴转动的角速度进行时间积分所得到的角度值作为所述六轴陀螺仪的零点漂移值；其中，所述静止状态是指机器人开机后没有移动前的状态或者机器人在移动过程中停止下来时的状态，且所述状态需要持续第二预定时间段。6.机器人基于六轴陀螺仪检测被卡的方法，其特征在于，包括如下步骤：步骤1：根据权利要求1至5任一项所述的机器人基于六轴陀螺仪检测仰角的方法，记录机器人检测到的仰角数据，并进入步骤2；步骤2：将第一预设时间段内所记录的仰角数据与机器人中预先存储的参考仰角数据进行比较，判断所记录的仰角数据是否在参考仰角数据的误差范围内，如果是，则进入步骤3，如果否，则将所述第一预设时间段内所记录的仰角数据中的第一个记录的数据删除，并返回步骤1；步骤3：持续记录机器人检测到的仰角数据，并判断所记录的仰角数据在参考仰角数据的误差范围内的持续时间是否达到第二预设时间段，如果是，则进入步骤4，如果否，则确定机器人没有被卡；步骤4：判断在所述第二预设时间段内是否检测...

【专利技术属性】
技术研发人员：李永勇，肖刚军，
申请(专利权)人：珠海市一微半导体有限公司，
类型：发明
国别省市：广东,44