CN112603202B - 离地检测方法、装置、移动机器人及存储介质 - Google Patents

离地检测方法、装置、移动机器人及存储介质 Download PDF

Info

Publication number
CN112603202B
CN112603202B CN202011417672.4A CN202011417672A CN112603202B CN 112603202 B CN112603202 B CN 112603202B CN 202011417672 A CN202011417672 A CN 202011417672A CN 112603202 B CN112603202 B CN 112603202B
Authority
CN
China
Prior art keywords
mobile robot
ground
max
robot
determining
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011417672.4A
Other languages
English (en)
Other versions
CN112603202A (zh
Inventor
钟名宏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Silver Star Intelligent Group Co Ltd
Original Assignee
Shenzhen Silver Star Intelligent Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Silver Star Intelligent Technology Co Ltd filed Critical Shenzhen Silver Star Intelligent Technology Co Ltd
Priority to CN202011417672.4A priority Critical patent/CN112603202B/zh
Publication of CN112603202A publication Critical patent/CN112603202A/zh
Application granted granted Critical
Publication of CN112603202B publication Critical patent/CN112603202B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/24—Floor-sweeping machines, motor-driven
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/28—Floor-scrubbing machines, motor-driven
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4002—Installations of electric equipment
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4011—Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及机器人技术领域，公开了离地检测方法、装置、移动机器人及存储介质，用于在无其他传感器的前提下，基于惯性测量单元IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。方法包括：预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；根据所述合角速度判断所述移动机器人是否处于悬空状态；当所述移动机器人处于悬空状态时，确定所述机器人离地。

Description

离地检测方法、装置、移动机器人及存储介质
技术领域
本发明涉及机器人技术领域，尤其涉及一种离地检测方法、装置、移动机器人及存储介质。
背景技术
轮式移动机器人在工作过程中，会由于各种原因被人为抱起，或被低矮障碍物卡住。扫地机需要检测出离地状态，以便及时暂停工作，保证使用安全，并作为重新规划工作路线的依据。
目前检测离地状态最常用的方法是在轮子处安装专用的传感器，通过检测轮子是否悬空来判断机器人的离地状态。另一类方法首先用竖直方向加速度的变化判断机器人是否有异常动作，再通过地检传感器检测悬空状态。这些方法的实现都需依赖专用传感器。
发明内容
本发明提供了一种离地检测方法、装置、移动机器人及存储介质，用于在无其他传感器的前提下，基于惯性测量单元IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
本发明实施例的第一方面提供一种离地检测方法，包括：预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；根据所述合角速度判断所述移动机器人是否处于悬空状态；当所述移动机器人处于悬空状态时，确定所述机器人离地。
在一种可行的实施方式中，所述根据所述合角速度判断所述移动机器人是否处于悬空状态，包括：对所述合角速度进行快速傅里叶变换，得到频率幅值谱；根据所述频率幅值谱判断所述移动机器人是否处于悬空状态。
在一种可行的实施方式中，所述根据所述频率幅值谱判断所述移动机器人是否处于悬空状态，包括：基于所述频率幅值谱确定零频幅值A0及低频区的幅值之和Alow_sum；判断所述A0和所述Alow_sum是否满足第一预置条件，所述第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益，T1为第一幅值阈值；若所述A0和所述Alow_sum满足所述第一预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第一预置条件，则基于所述频率幅值谱计算中频区最大幅值Amid_max和高频区最大幅值Ahigh_max；判断所述Amid_max和所述Ahigh_max是否满足第二预置条件，所述第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值；若所述Amid_max和所述Ahigh_max满足所述第二预置条件，则判断所述A0和所述Alow_sum是否满足第三预置条件，所述第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值，T3<T1；若所述A0和所述Alow_sum满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第四预置条件，所述第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益，K2<K1；若所述Amid_max和所述Ahigh_max满足所述第四预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第五预置条件，所述第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益,K3＞K1；若所述Amid_max和所述Ahigh_max满足所述第五预置条件，则确定所述移动机器人处于悬空状态。
在一种可行的实施方式中，所述根据所述频率幅值谱判断所述移动机器人是否处于悬空状态，还包括：若所述Amid_max和所述Ahigh_max不满足所述第二预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第四预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第五预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角。
在一种可行的实施方式中，在所述预判断移动机器人是否离地之前，还包括：获取加速度计Z轴读数Za，其中Za的单位为g；所述预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断，包括：计算所述加速度计Z轴读数Za与静置时Z轴方向上加速度Z0之间差值的绝对值，得到加速度差值；判断所述加速度差值是否大于加速度阈值且持续超过第一预置时长；若是，则预判断所述移动机器人离地；若否，则预判断所述移动机器人未离地。
在一种可行的实施方式中，在所述预判断移动机器人是否离地之前，还包括：计算移动机器人所在平面的地面倾斜角，所述地面倾斜角为移动机器人所在平面与水平面的夹角；获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在IMU坐标系Z轴方向上的加速度分量az，其中az的单位为g；根据所述az和预置倾斜角公式计算所述移动机器人所在平面与水平面的夹角，得到机身倾斜角θ，所述预置倾斜角公式为θ＝arccos(az)；所述预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断，包括：计算所述机身倾斜角θ与所述地面倾斜角之间差值的绝对值，得到倾斜角差值；判断所述倾斜角差值是否大于第一倾斜角阈值；若是，则预判断所述移动机器人离地；若否，则预判断所述移动机器人未离地。
在一种可行的实施方式中，所述获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，包括：读取所述移动机器人的姿态四元数q＝(q0,q1,q2,q3)；根据所述姿态四元数计算得到第一中间数T1、第二中间数T2和第三中间数T3，其中，T1＝2×(q1q3-q0q2)，T2＝2×(q2q3+q0q1)，
Figure BDA0002820709250000031
基于预置加速度公式计算当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，其中，所述预置加速度公式为
Figure BDA0002820709250000032
在一种可行的实施方式中，所述当所述移动机器人处于悬空状态时，确定所述机器人离地，包括：当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态；若所述移动机器人不处于所述静置状态，则确定所述机器人离地；若所述移动机器人处于所述静置状态，则确定所述机器人已落地。
在一种可行的实施方式中，所述当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态，包括：当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志；判断所述合角速度ω和机身倾斜角θ是否满足静置条件，所述静置条件为所述机身倾斜角θ小于第二倾斜角阈值，及所述合角速度ω小于合角速度阈值且持续超过第二预置时长；若所述合角速度ω和机身倾斜角θ不满足静置条件，则确定所述移动机器人未处于静置状态；若所述合角速度ω和机身倾斜角θ满足静置条件，则确定所述移动机器人处于静置状态。
在一种可行的实施方式中，在所述确定所述机器人已落地之后，所述离地检测方法还包括：当所述移动机器人已落地且处于静置状态时，获取所述移动机器人的当前状态信息并控制所述移动机器人根据所述当前状态信息进行运动。
在一种可行的实施方式中，所述当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度，包括：当预判断所述机器人离地时，获取所述移动机器人在所述IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；通过第一预置公式计算所述移动机器人在水平面上的合角速度，得到所述机器人IMU坐标系X轴和Y轴的合角速度ω，所述第一预置公式为
Figure BDA0002820709250000033
本发明实施例的第二方面提供了一种离地检测装置，应用于移动机器人，包括：预判断模块，用于预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；第一确定模块，用于当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；判断模块，用于根据所述合角速度判断所述移动机器人是否处于悬空状态；第二确定模块，用于当所述移动机器人处于悬空状态时，确定所述机器人离地。
在一种可行的实施方式中，判断模块包括：快速傅里叶变换单元，用于对所述合角速度进行快速傅里叶变换，得到频率幅值谱；离地判断单元，用于根据所述频率幅值谱判断所述移动机器人是否处于悬空状态。
在一种可行的实施方式中，所述离地判断单元具体用于：基于所述频率幅值谱确定零频幅值A0及低频区的幅值之和Alow_sum；判断所述A0和所述Alow_sum是否满足第一预置条件，所述第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益，T1为第一幅值阈值；若所述A0和所述Alow_sum满足所述第一预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第一预置条件，则基于所述频率幅值谱计算中频区最大幅值Amid_max和高频区最大幅值Ahigh_max；判断所述Amid_max和所述Ahigh_max是否满足第二预置条件，所述第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值；若所述Amid_max和所述Ahigh_max满足所述第二预置条件，则判断所述A0和所述Alow_sum是否满足第三预置条件，所述第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值，T3<T1；若所述A0和所述Alow_sum满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第四预置条件，所述第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益，K2<K1；若所述Amid_max和所述Ahigh_max满足所述第四预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第五预置条件，所述第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益,K3＞K1；若所述Amid_max和所述Ahigh_max满足所述第五预置条件，则确定所述移动机器人处于悬空状态。
在一种可行的实施方式中，所述离地判断单元具体还用于：若所述Amid_max和所述Ahigh_max不满足所述第二预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第四预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第五预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角。
在一种可行的实施方式中，所述离地检测装置还包括：第一获取模块，用于获取加速度计Z轴读数Za，其中Za的单位为g；所述预判断模块包括：计算单元，用于计算所述加速度计Z轴读数Za与静置时Z轴方向上加速度Z0之间差值的绝对值，得到加速度差值；阈值判断单元，用于判断所述加速度差值是否大于加速度阈值且持续超过第一预置时长；预判断单元，若是，则用于预判断所述移动机器人离地；所述预判断单元，若否，则用于预判断所述移动机器人未离地。
在一种可行的实施方式中，所述离地检测装置还包括：第一计算模块，用于计算移动机器人所在平面的地面倾斜角，所述地面倾斜角为移动机器人所在平面与水平面的夹角；第二获取模块，用于获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在IMU坐标系Z轴方向上的加速度分量az，其中az的单位为g；第二计算模块，用于根据所述az和预置倾斜角公式计算所述移动机器人所在平面与水平面的夹角，得到机身倾斜角θ，所述预置倾斜角公式为θ＝arccos(az)；所述预判断模块包括：所述计算单元，用于计算所述机身倾斜角θ与地面倾斜角之间差值的绝对值，得到倾斜角差值；所述阈值判断单元，用于判断所述倾斜角差值是否大于第一倾斜角阈值；所述预判断单元，若是，则用于预判断所述移动机器人离地；所述预判断单元，若否，则用于预判断所述移动机器人未离地。
在一种可行的实施方式中，所述第二获取模块具体用于：读取所述移动机器人的姿态四元数q＝(q0,q1,q2,q3)；根据所述姿态四元数计算得到第一中间数T1、第二中间数T2和第三中间数T3，其中，T1＝2×(q1q3-q0q2)，T2＝2×(q2q3+q0q1)，
Figure BDA0002820709250000051
基于预置加速度公式计算当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，其中，所述预置加速度公式为
Figure BDA0002820709250000052
在一种可行的实施方式中，第二确定模块包括：激活判断单元，用于当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态；确定单元，若所述移动机器人不处于所述静置状态，则用于确定所述机器人离地；所述确定单元，若所述移动机器人处于所述静置状态，则用于确定所述机器人已落地。
在一种可行的实施方式中，激活判断单元具体用于：当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志；判断所述合角速度ω和机身倾斜角θ是否满足静置条件，所述静置条件为所述机身倾斜角θ小于第二倾斜角阈值，及所述合角速度ω小于合角速度阈值且持续超过第二预置时长；若所述合角速度ω和机身倾斜角θ不满足静置条件，则确定所述移动机器人未处于静置状态；若所述合角速度ω和机身倾斜角θ满足静置条件，则确定所述移动机器人处于静置状态。
在一种可行的实施方式中，第二确定模块还包括：控制运动单元，用于当所述移动机器人已落地且处于静置状态时，获取所述移动机器人的当前状态信息并控制所述移动机器人根据所述当前状态信息进行运动。
在一种可行的实施方式中，第一确定模块具体用于：当预判断所述机器人离地时，获取所述移动机器人在所述IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；通过第一预置公式计算所述移动机器人在水平面上的合角速度，得到所述机器人IMU坐标系X轴和Y轴的合角速度ω，所述第一预置公式为
Figure BDA0002820709250000053
本发明实施例的第三方面提供了一种移动机器人，包括：存储器和至少一个处理器，所述存储器中存储有指令；所述至少一个处理器调用所述存储器中的所述指令，以使得所述移动机器人执行上述的离地检测方法。
本发明的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述的离地检测方法。
本发明实施例提供的技术方案中，预判断移动机器人是否离地，预判断包括根据加速度计Z轴读数和/或根据移动机器人的机身倾斜角判断；当预判断机器人离地时，确定机器人IMU坐标系X轴和Y轴的合角速度；根据合角速度判断移动机器人是否处于悬空状态；当移动机器人处于悬空状态时，确定机器人离地。本发明实施例，在无其他传感器的前提下，基于IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
附图说明
图1为本发明实施例中离地检测方法的一个实施例示意图；
图2为本发明实施例中判断悬空状态的流程示意图；
图3为本发明实施例中移动机器人在正常工作状态下X和Y轴合成角速度及FFT变化结果的一个示意图；
图4为本发明实施例中移动机器人悬空且人为动作较明显时X和Y轴合成角速度及FFT变化结果的一个示意图；
图5为本发明实施例中移动机器人悬空且人为动作不明显时X和Y轴合成角速度及FFT变化结果的另一个示意图；
图6为本发明实施例中离地检测方法的另一个实施例示意图；
图7为本发明实施例中移动机器人被双手平抬起再放回地面过程中加速度计Z轴读数的变化示意图；
图8为本发明实施例中移动机器人被单手拎起再放回地面过程中机身倾斜角的变化示意图；
图9为本发明实施例中移动机器人被双手平抬起再放回地面过程中加速度计Z轴读数和检测结果的一个示意图；
图10为本发明实施例中移动机器人被单手提起过程中X和Y轴合成角速度和检测结果的一个示意图；
图11为本发明实施例中移动机器人从平地运动至斜面一段时间后再回到平地过程中X和Y轴合成角速度和检测结果的一个示意图；
图12为本发明实施例中离地检测装置的一个实施例示意图；
图13为本发明实施例中移动机器人的一个实施例示意图。
具体实施方式
本发明提供了一种离地检测方法、装置、移动机器人及存储介质，用于在无其他传感器的前提下，基于IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
可以理解的是，所述移动机器人可以是扫地机器人、扫拖一体式机器人、擦地机器人或洗地机器人等的任意一种。
请参阅图1，本发明实施例提供的离地检测方法的流程图，具体包括：
101、预判断移动机器人是否离地，预判断包括根据加速度计Z轴读数和/或根据移动机器人的机身倾斜角判断。
移动机器人通过内置的控制器预判断移动机器人是否离地，预判断包括根据加速度计Z轴读数和/或根据移动机器人的机身倾斜角判断。
需要说明的是，移动机器人可以只根据加速度计Z轴读数预判断移动机器人是否离地，也可以只根据移动机器人的机身倾斜角预判断移动机器人是否离地，或者是同时根据加速度计Z轴读数和机身倾斜角预判断移动机器人是否离地。其中，倾斜角是指移动机器人底面所在的平面与水平面之间的角度。
102、当预判断机器人离地时，确定机器人IMU坐标系X轴和Y轴的合角速度。
当预判断机器人离地时，移动机器人确定机器人IMU坐标系X轴和Y轴的合角速度。具体的，当预判断机器人离地时，移动机器人获取移动机器人在IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；移动机器人通过第一预置公式计算移动机器人在水平面上的合角速度，得到机器人IMU坐标系X轴和Y轴的合角速度ω，第一预置公式为
Figure BDA0002820709250000071
需要说明的是，移动机器人在正常工作(除转弯、碰撞)时，陀螺仪读数通常只会产生振幅很小的振动，而当被人为抱起(即离地)时，竖直向上的加速度(即z轴加速度)会发生快速变化，有时还会出现明显的机身倾斜。悬空时，会因人的动作而产生振幅较大的低频振动，当机器人冲上低矮障碍物被卡住悬空时，其z轴加速度和角速度表现与被人为抱起的情况相似(即离地)。而不直接使用z轴加速度读数计算倾斜角的原因是，加速度计本身的噪声、机身震动、以及在非水平面上运动时引起的线加速度都会给计算出的倾斜角带来明显的噪声和误差。
103、根据合角速度判断移动机器人是否处于悬空状态。
移动机器人根据合角速度判断移动机器人是否处于悬空状态。具体的，移动机器人对合角速度进行快速傅里叶变换，得到频率幅值谱；移动机器人根据频率幅值谱判断移动机器人是否处于悬空状态。
其中，移动机器人根据频率幅值谱判断移动机器人是否处于悬空状态的过程具体包括：参考图2，移动机器人基于频率幅值谱确定零频幅值A0及低频区(指频率幅值谱中频率为1～10Hz的区域)的幅值之和Alow_sum(即对1～10Hz中每个频率对应的幅值求和)；移动机器人判断A0和Alow_sum是否满足第一预置条件，第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益(K1取值可以为1.0)，T1为第一幅值阈值(T1取值可以为0.5)；若A0和Alow_sum满足第一预置条件，则移动机器人确定移动机器人处于悬空状态；若A0和Alow_sum不满足第一预置条件，则移动机器人基于频率幅值谱计算中频区(指频率幅值谱中频率为40～80Hz的区域)最大幅值Amid_max和高频区(指频率幅值谱中频率为81～100Hz的区域)最大幅值Ahigh_max；移动机器人判断Amid_max和Ahigh_max是否满足第二预置条件，第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值(T2取值可以为0.01)；若Amid_max和Ahigh_max不满足第二预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角；若Amid_max和Ahigh_max满足第二预置条件，则移动机器人判断A0和Alow_sum是否满足第三预置条件，第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值(T3取值可以为0.1)，T3<T1；若A0和Alow_sum满足第三预置条件，则移动机器人判断Amid_max和Ahigh_max是否满足第四预置条件，第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益(K2取值可以为0.9)，K2<K1；若Amid_max和Ahigh_max满足第四预置条件，则移动机器人确定移动机器人处于悬空状态；若Amid_max和Ahigh_max不满足第四预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角；若A0和Alow_sum不满足第三预置条件，则移动机器人判断Amid_max和Ahigh_max是否满足第五预置条件，第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益(K3取值可以为1.5),K3＞K1；若Amid_max和Ahigh_max满足第五预置条件，则移动机器人确定移动机器人处于悬空状态；若Amid_max和Ahigh_max不满足第五预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角。
其中，移动机器人在正常工作时，即当移动机器人处于正常工作状态时，移动机器人X和Y轴的合成角速度及FFT变换结果如图3所示；而当移动机器人悬空且人为动作较明显时，移动机器人X和Y轴的合成角速度及FFT变换结果如图4所示，当移动机器人悬空且人为动作不明显时，移动机器人X和Y轴的合成角速度及FFT变换结果如图5所示。需要说明的是，各参数(包括K1、K2、K3、T1、T2、T3)的选取与IMU的设置和移动机器人结构有关，可以根据移动机器人选取合适的参数。
104、当移动机器人处于悬空状态时，确定机器人离地。
具体的，当移动机器人处于悬空状态时，激活移动机器人的离地标志，并基于合角速度判断移动机器人是否处于静置状态；若移动机器人不处于静置状态，则确定机器人离地，不做处理，继续判断移动机器人是否处于静置状态；若移动机器人处于静置状态，则确定机器人已落地。
本发明实施例，在无其他传感器的前提下，基于IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
请参阅图6，本发明实施例提供的离地检测方法的另一个流程图，具体包括：
601、预判断移动机器人是否离地，预判断包括根据加速度计Z轴读数和/或根据移动机器人的机身倾斜角判断。
移动机器人通过内置的控制器预判断移动机器人是否离地，预判断包括根据加速度计Z轴读数和/或根据移动机器人的机身倾斜角判断。
需要说明的是，移动机器人可以只根据加速度计Z轴读数预判断移动机器人是否离地，也可以只根据移动机器人的机身倾斜角预判断移动机器人是否离地，或者是同时根据加速度计Z轴读数和机身倾斜角预判断移动机器人是否离地。其中，倾斜角是指移动机器人底面所在的平面与水平面之间的角度。
例如，若只根据加速度计Z轴读数预判断移动机器人是否离地，具体过程如下：计算加速度计Z轴读数Za与静置时Z轴方向上加速度Z0之间差值的绝对值，得到加速度差值；判断加速度差值是否大于加速度阈值(例如，阈值为0.15g)且持续超过第一预置时长(例如，0.03s)；若是，则预判断移动机器人离地；若否，则预判断移动机器人未离地。例如，移动机器人被双手平抬起再放回地面过程中加速度计Z轴读数的变化情况如图7所示。可以理解的是，在预判断移动机器人是否离地之前，还包括：获取加速度计Z轴读数Za，其中Za的单位为g。
又例如，若只根据机身倾斜角预判断移动机器人是否离地，具体过程如下：计算机身倾斜角θ与地面倾斜角之间差值的绝对值，得到倾斜角差值；判断倾斜角差值是否大于第一倾斜角阈值(例如，10°)；若是，则预判断移动机器人离地；若否，则预判断移动机器人未离地。例如，移动机器人被单手拎起再放回地面过程中的机身倾斜角的变化情况如图8所示。可以理解的是，在预判断移动机器人是否离地之前，还包括：计算移动机器人所在平面的地面倾斜角，地面倾斜角为移动机器人所在平面与水平面的夹角；获取移动机器人的姿态四元数，并根据姿态四元数确定当前重力加速度在IMU坐标系Z轴方向上的加速度分量az，其中az的单位为g；根据az和预置倾斜角公式计算移动机器人所在平面与水平面的夹角，得到机身倾斜角θ，预置倾斜角公式为θ＝arccos(az)。
需要说明的是，本实施例中，获取移动机器人的姿态四元数，并根据姿态四元数确定当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，包括：读取移动机器人的姿态四元数q＝(q0,q1,q2,q3)；根据姿态四元数计算得到第一中间数T1、第二中间数T2和第三中间数T3，其中，T1＝2×(q1q3-q0q2)，T2＝2×(q2q3+q0q1)，
Figure BDA0002820709250000091
基于预置加速度公式计算当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，其中，预置加速度公式为
Figure BDA0002820709250000092
602、当预判断机器人离地时，确定机器人IMU坐标系X轴和Y轴的合角速度。
当预判断机器人离地时，移动机器人确定机器人IMU坐标系X轴和Y轴的合角速度。具体的，当预判断机器人离地时，移动机器人获取移动机器人在IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；移动机器人通过第一预置公式计算移动机器人在水平面上的合角速度，得到机器人IMU坐标系X轴和Y轴的合角速度ω，第一预置公式为
Figure BDA0002820709250000101
需要说明的是，移动机器人在正常工作(除转弯、碰撞)时，陀螺仪读数通常只会产生振幅很小的振动，而当被人为抱起(即离地)时，竖直向上的加速度(即z轴加速度)会发生快速变化，有时还会出现明显的机身倾斜。悬空时，会因人的动作而产生振幅较大的低频振动，当机器人冲上低矮障碍物被卡住悬空时，其z轴加速度和角速度表现与被人为抱起的情况相似(即离地)。而不直接使用z轴加速度读数计算倾斜角的原因是，加速度计本身的噪声、机身震动、以及在非水平面上运动时引起的线加速度都会给计算出的倾斜角带来明显的噪声和误差。
603、根据合角速度判断移动机器人是否处于悬空状态。
移动机器人根据合角速度判断移动机器人是否处于悬空状态。具体的，移动机器人对合角速度进行快速傅里叶变换，得到频率幅值谱；移动机器人根据频率幅值谱判断移动机器人是否处于悬空状态。
其中，移动机器人根据频率幅值谱判断移动机器人是否处于悬空状态的过程具体包括：同样参考图2，移动机器人基于频率幅值谱确定零频幅值A0及低频区(指频率幅值谱中频率为1～10Hz的区域)的幅值之和Alow_sum(即对1～10Hz中每个频率对应的幅值求和)；移动机器人判断A0和Alow_sum是否满足第一预置条件，第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益(K1取值可以为1.0)，T1为第一幅值阈值(T1取值可以为0.5)；若A0和Alow_sum满足第一预置条件，则移动机器人确定移动机器人处于悬空状态；若A0和Alow_sum不满足第一预置条件，则移动机器人基于频率幅值谱计算中频区(指频率幅值谱中频率为40～80Hz的区域)最大幅值Amid_max和高频区(指频率幅值谱中频率为81～100Hz的区域)最大幅值Ahigh_max；移动机器人判断Amid_max和Ahigh_max是否满足第二预置条件，第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值(T2取值可以为0.01)；若Amid_max和Ahigh_max不满足第二预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角；若Amid_max和Ahigh_max满足第二预置条件，则移动机器人判断A0和Alow_sum是否满足第三预置条件，第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值(T3取值可以为0.1)，T3<T1；若A0和Alow_sum满足第三预置条件，则移动机器人判断Amid_max和Ahigh_max是否满足第四预置条件，第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益(K2取值可以为0.9)，K2<K1；若Amid_max和Ahigh_max满足第四预置条件，则移动机器人确定移动机器人处于悬空状态；若Amid_max和Ahigh_max不满足第四预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角；若A0和Alow_sum不满足第三预置条件，则移动机器人判断Amid_max和Ahigh_max是否满足第五预置条件，第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益(K3取值可以为1.5),K3＞K1；若Amid_max和Ahigh_max满足第五预置条件，则移动机器人确定移动机器人处于悬空状态；若Amid_max和Ahigh_max不满足第五预置条件，则确定移动机器人未处于悬空状态，更新地面倾斜角。
需要说明的是，若确定移动机器人未处于悬空状态，则更新地面倾斜角，并重复执行步骤601，具体此处不再赘述。
其中，移动机器人在正常工作时，即当移动机器人处于正常工作状态时，移动机器人X和Y轴的合成角速度及FFT变换结果如图3所示；而当移动机器人悬空且人为动作较明显时，移动机器人X和Y轴的合成角速度及FFT变换结果如图4所示，当移动机器人悬空且人为动作不明显时，移动机器人X和Y轴的合成角速度及FFT变换结果如图5所示。需要说明的是，各参数(包括K1、K2、K3、T1、T2、T3)的选取与IMU的设置和移动机器人结构有关，可以根据移动机器人选取合适的参数。
604、当移动机器人处于悬空状态时，激活移动机器人的离地标志，并基于合角速度判断移动机器人是否处于静置状态。
具体的，当移动机器人处于悬空状态时，激活移动机器人的离地标志；判断合角速度ω和机身倾斜角θ是否满足静置条件，静置条件为机身倾斜角θ小于第二倾斜角阈值，及合角速度ω小于合角速度阈值且持续超过第二预置时长；若合角速度ω和机身倾斜角θ不满足静置条件，则确定移动机器人未处于静置状态，不做处理，继续判断移动机器人是否满足静置条件；若合角速度ω和机身倾斜角θ满足静置条件，则确定移动机器人处于静置状态。
可以理解的是，当移动机器人不满足静置条件，则可以确定移动机器人为悬空状态，继续判断移动机器人是否满足静置条件，当移动机器人满足静置条件，则可以确定移动机器人为已经放回地面，可以重新进行工作。
605、若移动机器人不处于静置状态，则确定机器人离地。
若移动机器人不处于静置状态，则确定移动机器人离地，保持离地标志处于激活状态，继续执行步骤604。
需要说明的是，对于不同场景下，加速度计Z轴读数的表现也不同，例如，(1)当移动机器人被双手平抬起再放回地面时，加速度计Z轴读数和检测结果如图9所示；(2)当移动机器人被单手提起时，x、y轴合角速度和检测结果如图10所示；(3)移动机器人从平地运动至斜面，在斜面上运动一段时间后再回到平地过程中，x、y轴合角速度和检测结果如图11所示。图9、图10中的虚线矩形波和实线矩形波的上升沿时间差表示移动机器人从满足预判条件到满足悬空条件的判断时间间隔，并且移动机器人被抱起瞬间到满足预判断条件(即虚线矩形波处于上升沿)时已经有0.1～0.3秒的预判断延迟，预判断延迟加上判断时间间隔，即得到检测延时，其中，虚线矩形波在较低处表示移动机器人不满足预判断条件，虚线矩形波在较高处表示移动机器人满足预判断条件，而该预判断条件表示移动机器人可能被抱起或离地，实线矩形波在较低处表示确定移动机器人未被抱起或已放回地面，实线矩形波在较高处表示确定移动机器人被抱起或离地。同理，图11中的虚线矩形波在较低处表示移动机器人不满足预判断条件，虚线矩形波在较高处表示移动机器人满足预判断条件，而实线矩形波一直处于较低处，表示确定移动机器人未被抱起或离地，实际上移动机器人也确实没有离地，只是运动到一个斜面上并返回。
606、若移动机器人处于静置状态，则确定机器人已落地。
当移动机器人已落地且处于静置状态时，将离地标志从激活状态调整为关闭状态，获取移动机器人的当前状态信息并控制移动机器人根据当前状态信息进行运动。例如，当机器人处于悬空状态，且已停止运动时，检查机身倾斜角度和合角速度，如果机身倾斜角度小于某阈值(例如30°)，且一段时间(例如0.5s)内均为静止状态(例如合角速度小于0.02rad/s)，则认为机器人已经被放回地面并处于静置状态，移动机器人获取状态信息后将重新开始运动。
本发明实施例，在无其他传感器的前提下，基于惯性测量单元IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
上面对本发明实施例中离地检测方法进行了描述，下面对本发明实施例中离地检测装置进行描述，请参阅图12，本发明实施例中离地检测装置的一个实施例包括：
预判断模块1201，用于预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；
第一确定模块1202，用于当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；
判断模块1203，用于根据所述合角速度判断所述移动机器人是否处于悬空状态；
第二确定模块1204，用于当所述移动机器人处于悬空状态时，确定所述机器人离地。
可选的，判断模块1203包括：
快速傅里叶变换单元12031，用于对所述合角速度进行快速傅里叶变换，得到频率幅值谱；
离地判断单元12032，用于根据所述频率幅值谱判断所述移动机器人是否处于悬空状态。
可选的，离地判断单元12032具体用于：
基于所述频率幅值谱确定零频幅值A0及低频区的幅值之和Alow_sum；判断所述A0和所述Alow_sum是否满足第一预置条件，所述第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益，T1为第一幅值阈值；若所述A0和所述Alow_sum满足所述第一预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第一预置条件，则基于所述频率幅值谱计算中频区最大幅值Amid_max和高频区最大幅值Ahigh_max；判断所述Amid_max和所述Ahigh_max是否满足第二预置条件，所述第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值；若所述Amid_max和所述Ahigh_max满足所述第二预置条件，则判断所述A0和所述Alow_sum是否满足第三预置条件，所述第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值，T3<T1；若所述A0和所述Alow_sum满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第四预置条件，所述第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益，K2<K1；若所述Amid_max和所述Ahigh_max满足所述第四预置条件，则确定所述移动机器人处于悬空状态；若所述A0和所述Alow_sum不满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第五预置条件，所述第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益,K3＞K1；若所述Amid_max和所述Ahigh_max满足所述第五预置条件，则确定所述移动机器人处于悬空状态。
可选的，离地判断单元12032具体用于：
若所述Amid_max和所述Ahigh_max不满足所述第二预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第四预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；若所述Amid_max和所述Ahigh_max不满足所述第五预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角。
可选的，所述离地检测装置还包括：
第一获取模块1205，用于获取加速度计Z轴读数Za，其中Za的单位为g；
预判断模块1201包括：
计算单元12011，用于计算加速度计Z轴读数Za与静置时Z轴方向上加速度Z0之间差值的绝对值，得到加速度差值；
阈值判断单元12012，用于判断所述加速度差值是否大于加速度阈值且持续超过第一预置时长；
预判断单元12013，若是，则用于预判断所述移动机器人离地；
所述预判断单元12013，若否，则用于预判断所述移动机器人未离地。
可选的，所述离地检测装置还包括：
第一计算模块1206，用于计算移动机器人所在平面的地面倾斜角，所述地面倾斜角为移动机器人所在平面与水平面的夹角；
第二获取模块1207，用于获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在IMU坐标系Z轴方向上的加速度分量az，其中az的单位为g；
第二计算模块1208，用于根据所述az和预置倾斜角公式计算所述移动机器人所在平面与水平面的夹角，得到机身倾斜角θ，所述预置倾斜角公式为θ＝arccos(az)；
所示预判断模块1201包括：
所述计算单元12011，用于计算所述机身倾斜角θ与地面倾斜角之间差值的绝对值，得到倾斜角差值；
所述阈值判断单元12012，用于判断所述倾斜角差值是否大于第一倾斜角阈值；
所述预判断单元12013，若是，则用于预判断所述移动机器人离地；
所述预判断单元12013，若否，则用于预判断所述移动机器人未离地。
可选的，第二获取模块1207具体用于：
读取所述移动机器人的姿态四元数q＝(q0,q1,q2,q3)；根据所述姿态四元数计算得到第一中间数T1、第二中间数T2和第三中间数T3，其中，T1＝2×(q1q3-q0q2)，T2＝2×(q2q3+q0q1)，
Figure BDA0002820709250000141
基于预置加速度公式计算当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，其中，所述预置加速度公式为
Figure BDA0002820709250000142
可选的，第二确定模块1204包括：
激活判断单元12041，用于当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态；
确定单元12042，若所述移动机器人不处于所述静置状态，则用于确定所述机器人离地；
所述确定单元12042，若所述移动机器人处于所述静置状态，则用于确定所述机器人已落地。
可选的，激活判断单元12041具体用于：
当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志；判断所述合角速度ω和机身倾斜角θ是否满足静置条件，所述静置条件为所述机身倾斜角θ小于第二倾斜角阈值，及所述合角速度ω小于合角速度阈值且持续超过第二预置时长；若所述合角速度ω和机身倾斜角θ不满足静置条件，则确定所述移动机器人未处于静置状态；若所述合角速度ω和机身倾斜角θ满足静置条件，则确定所述移动机器人处于静置状态。
可选的，第二确定模块1204包括：
控制运动单元12043，用于当所述移动机器人已落地且处于静置状态时，获取所述移动机器人的当前状态信息并控制所述移动机器人根据所述当前状态信息进行运动。
可选的，第一确定模块1202具体用于：
当预判断所述机器人离地时，获取所述移动机器人在所述IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；通过第一预置公式计算所述移动机器人在水平面上的合角速度，得到所述机器人IMU坐标系X轴和Y轴的合角速度ω，所述第一预置公式为
Figure BDA0002820709250000151
本发明实施例，在无其他传感器的前提下，基于惯性测量单元IMU读取的信息准确地检测移动机器人是否离地，降低了移动机器人的结构复杂度和硬件成本。
图13是本发明实施例提供的一种移动机器人的结构示意图，该移动机器人1300可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(centralprocessing units，CPU)1310(例如，一个或一个以上处理器)和存储器1320，一个或一个以上存储应用程序1333或数据1332的存储介质1330(例如一个或一个以上海量存储设备)。其中，存储器1320和存储介质1330可以是短暂存储或持久存储。存储在存储介质1330的程序可以包括一个或一个以上模块(图示没标出)，每个模块可以包括对移动机器人1300中的一系列指令操作。更进一步地，处理器1310可以设置为与存储介质1330通信，在移动机器人1300上执行存储介质1330中的一系列指令操作。
可以理解的是，所述移动机器人可以是扫地机器人、扫拖一体式机器人、擦地机器人或洗地机器人等的任意一种。
本发明还提供一种计算机可读存储介质，该计算机可读存储介质可以为非易失性计算机可读存储介质，该计算机可读存储介质也可以为易失性计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在计算机上运行时，使得计算机执行所述离地检测方法的步骤。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(read-only memory，ROM)、随机存取存储器(random access memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (13)

1.一种离地检测方法，应用于移动机器人，其特征在于，包括：
预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；
当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；
根据所述合角速度判断所述移动机器人是否处于悬空状态；
当所述移动机器人处于悬空状态时，确定所述机器人离地；
所述根据所述合角速度判断所述移动机器人是否处于悬空状态，包括：
对所述合角速度进行快速傅里叶变换，得到频率幅值谱；
根据所述频率幅值谱判断所述移动机器人是否处于悬空状态。
2.根据权利要求1所述的离地检测方法，其特征在于，所述根据所述频率幅值谱判断所述移动机器人是否处于悬空状态，包括：
基于所述频率幅值谱确定零频幅值A0及低频区的幅值之和Alow_sum；
判断所述A0和所述Alow_sum是否满足第一预置条件，所述第一预置条件为A0×K1+Alow_sum≥T1，其中，K1为第一增益，T1为第一幅值阈值；
若所述A0和所述Alow_sum满足所述第一预置条件，则确定所述移动机器人处于悬空状态；
若所述A0和所述Alow_sum不满足所述第一预置条件，则基于所述频率幅值谱计算中频区最大幅值Amid_max和高频区最大幅值Ahigh_max；
判断所述Amid_max和所述Ahigh_max是否满足第二预置条件，所述第二预置条件为Amid_max≤T2且Ahigh_max≤T2，其中，T2为第二幅值阈值；
若所述Amid_max和所述Ahigh_max满足所述第二预置条件，则判断所述A0和所述Alow_sum是否满足第三预置条件，所述第三预置条件为A0≥T3或Alow_sum≥T3，其中，T3为第三幅值阈值，T3<T1；
若所述A0和所述Alow_sum满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第四预置条件，所述第四预置条件为Amid_max×K2<Ahigh_max，其中，K2为第二增益，K2<K1；
若所述Amid_max和所述Ahigh_max满足所述第四预置条件，则确定所述移动机器人处于悬空状态；
若所述A0和所述Alow_sum不满足所述第三预置条件，则判断所述Amid_max和所述Ahigh_max是否满足第五预置条件，所述第五预置条件为Amid_max×K3<Ahigh_max，其中，K3为第三增益,K3>K1；
若所述Amid_max和所述Ahigh_max满足所述第五预置条件，则确定所述移动机器人处于悬空状态。
3.根据权利要求2所述的离地检测方法，其特征在于，所述根据所述频率幅值谱判断所述移动机器人是否处于悬空状态，还包括：
若所述Amid_max和所述Ahigh_max不满足所述第二预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；
若所述Amid_max和所述Ahigh_max不满足所述第四预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角；
若所述Amid_max和所述Ahigh_max不满足所述第五预置条件，则确定所述移动机器人未处于悬空状态，更新地面倾斜角。
4.根据权利要求1所述的离地检测方法，其特征在于，在所述预判断移动机器人是否离地之前，还包括：
获取加速度计Z轴读数Za，其中Za的单位为g；
所述预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断，包括：
计算所述加速度计Z轴读数Za与静置时Z轴方向上加速度Z0之间差值的绝对值，得到加速度差值；
判断所述加速度差值是否大于加速度阈值且持续超过第一预置时长；
若是，则预判断所述移动机器人离地；
若否，则预判断所述移动机器人未离地。
5.根据权利要求1所述的离地检测方法，其特征在于，在所述预判断移动机器人是否离地之前，还包括：
计算移动机器人所在平面的地面倾斜角，所述地面倾斜角为移动机器人所在平面与水平面的夹角；
获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在IMU坐标系Z轴方向上的加速度分量az，其中az的单位为g；
根据所述az和预置倾斜角公式计算所述移动机器人所在平面与水平面的夹角，得到机身倾斜角θ，所述预置倾斜角公式为θ＝arccos(az)；
所述预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断，包括：
计算所述机身倾斜角θ与所述地面倾斜角之间差值的绝对值，得到倾斜角差值；
判断所述倾斜角差值是否大于第一倾斜角阈值；
若是，则预判断所述移动机器人离地；
若否，则预判断所述移动机器人未离地。
6.根据权利要求5所述的离地检测方法，其特征在于，所述获取移动机器人的姿态四元数，并根据所述姿态四元数确定当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，包括：
读取所述移动机器人的姿态四元数q＝(q0,q1,q2,q3)；
根据所述姿态四元数计算得到第一中间数T1、第二中间数T2和第三中间数T3，其中，T1＝2×(q1q3-q0q2)，T2＝2×(q2q3+q0q1)，
Figure FDA0003223615540000031
基于预置加速度公式计算当前重力加速度在预置的IMU坐标系Z轴方向上的加速度分量az，其中，所述预置加速度公式为
Figure FDA0003223615540000032
7.根据权利要求1所述的离地检测方法，其特征在于，所述当所述移动机器人处于悬空状态时，确定所述机器人离地，包括：
当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态；
若所述移动机器人不处于所述静置状态，则确定所述机器人离地；
若所述移动机器人处于所述静置状态，则确定所述机器人已落地。
8.根据权利要求7所述的离地检测方法，其特征在于，所述当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志，并基于所述合角速度判断所述移动机器人是否处于静置状态，包括：
当所述移动机器人处于悬空状态时，激活所述移动机器人的离地标志；
判断所述合角速度ω和机身倾斜角θ是否满足静置条件，所述静置条件为所述机身倾斜角θ小于第二倾斜角阈值，及所述合角速度ω小于合角速度阈值且持续超过第二预置时长；
若所述合角速度ω和机身倾斜角θ不满足静置条件，则确定所述移动机器人未处于静置状态；
若所述合角速度ω和机身倾斜角θ满足静置条件，则确定所述移动机器人处于静置状态。
9.根据权利要求7所述的离地检测方法，其特征在于，在所述确定所述机器人已落地之后，所述离地检测方法还包括：
当所述移动机器人已落地且处于静置状态时，获取所述移动机器人的当前状态信息并控制所述移动机器人根据所述当前状态信息进行运动。
10.根据权利要求1-9中任意一项所述的离地检测方法，其特征在于，所述当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度，包括：
当预判断所述机器人离地时，获取所述移动机器人在所述IMU坐标系X轴方向的角速度ωx和Y轴方向的角速度ωy；
通过第一预置公式计算所述移动机器人在水平面上的合角速度，得到所述机器人IMU坐标系X轴和Y轴的合角速度ω，所述第一预置公式为
Figure FDA0003223615540000033
11.一种离地检测装置，应用于移动机器人，其特征在于，包括：
预判断模块，用于预判断移动机器人是否离地，所述预判断包括根据加速度计Z轴读数和/或根据所述移动机器人的机身倾斜角判断；
第一确定模块，用于当预判断所述机器人离地时，确定所述机器人IMU坐标系X轴和Y轴的合角速度；
判断模块，用于根据所述合角速度判断所述移动机器人是否处于悬空状态；所述根据所述合角速度判断所述移动机器人是否处于悬空状态，包括：对所述合角速度进行快速傅里叶变换，得到频率幅值谱；根据所述频率幅值谱判断所述移动机器人是否处于悬空状态；
第二确定模块，用于当所述移动机器人处于悬空状态时，确定所述机器人离地。
12.一种移动机器人，其特征在于，所述移动机器人包括：存储器和至少一个处理器，所述存储器中存储有指令，所述存储器和所述至少一个处理器通过线路互连；
所述至少一个处理器调用所述存储器中的所述指令，以使得所述移动机器人执行如权利要求1-10中任意一项所述的离地检测方法。
13.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有指令，当所述指令被处理器执行时实现如权利要求1-10中任意一项所述的离地检测方法。
CN202011417672.4A 2020-12-07 2020-12-07 离地检测方法、装置、移动机器人及存储介质 Active CN112603202B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011417672.4A CN112603202B (zh) 2020-12-07 2020-12-07 离地检测方法、装置、移动机器人及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011417672.4A CN112603202B (zh) 2020-12-07 2020-12-07 离地检测方法、装置、移动机器人及存储介质

Publications (2)

Publication Number Publication Date
CN112603202A CN112603202A (zh) 2021-04-06
CN112603202B true CN112603202B (zh) 2022-01-11

Family

ID=75229153

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011417672.4A Active CN112603202B (zh) 2020-12-07 2020-12-07 离地检测方法、装置、移动机器人及存储介质

Country Status (1)

Country Link
CN (1) CN112603202B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11543245B1 (en) * 2022-04-14 2023-01-03 ALMA Technologies Ltd. System and method for estimating a location of a vehicle using inertial sensors

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1127402C (zh) * 1998-11-30 2003-11-12 索尼公司 机器人装置及其控制方法
KR101297388B1 (ko) * 2006-06-16 2013-08-19 삼성전자주식회사 위치 보정 기능을 제공하는 이동 장치 및 위치 보정 방법
CN106737549B (zh) * 2016-12-21 2019-05-31 深圳中智科创机器人有限公司 一种移动机器人及其防倾覆方法和装置
JP6993218B2 (ja) * 2017-12-26 2022-01-13 株式会社北川鉄工所 吊荷制御方法

Also Published As

Publication number Publication date
CN112603202A (zh) 2021-04-06

Similar Documents

Publication Publication Date Title
US10975545B2 (en) Work equipment control device and work machine
CN112731953B (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN113064433B (zh) 机器人迈步控制方法、装置、机器人控制设备及存储介质
CN111002346B (zh) 一种机器人被困检测方法以及机器人
CN113031639B (zh) 机器人步长处理方法、装置、机器人控制设备及存储介质
AU2015305864B2 (en) Earthmoving machine comprising weighted state estimator
CN112603202B (zh) 离地检测方法、装置、移动机器人及存储介质
KR20150006592A (ko) 마찰계수 추정 기능을 갖는 이동 로봇 및 마찰계수 추정 방법
US20190120627A1 (en) Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus
CN111650955B (zh) 一种攀爬机器人的控制方法及攀爬机器人
JP2008129813A (ja) 柔軟アームを有する移動ロボット
JP2008089517A (ja) 方位判別装置、方位判別方法及び方位判別プログラム
CN110132271A (zh) 一种自适应卡尔曼滤波姿态估计算法
US20190286159A1 (en) Position estimation system, position detection method, and program
JP2011059020A (ja) 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム
CN104546391B (zh) 一种用于盲杖的陀螺稳定装置及其互补滤波方法
US10466054B2 (en) Method and system for estimating relative angle between headings
CN104819717A (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN110293557A (zh) 姿势角计算装置、移动装置、姿势角计算方法和程序
JP6454856B2 (ja) 姿勢検出装置及び姿勢検出方法
CN109674480B (zh) 一种基于改进互补滤波的人体运动姿态解算方法
JP2021099384A5 (zh)
CN112346468B (zh) 一种自动引导运输车运动监控方法、系统及电子设备
CN106289642B (zh) 起吊工况下救援车整体重心平面位置的动态估计方法
JP2015215179A (ja) 磁気検知システムおよび磁気検知方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: 518110 1701, building 2, Yinxing Zhijie, No. 1301-72, sightseeing Road, Xinlan community, Guanlan street, Longhua District, Shenzhen, Guangdong Province

Patentee after: Shenzhen Yinxing Intelligent Group Co.,Ltd.

Address before: 518110 Building A1, Yinxing Hi-tech Industrial Park, Guanlan Street Sightseeing Road, Longhua District, Shenzhen City, Guangdong Province

Patentee before: Shenzhen Silver Star Intelligent Technology Co.,Ltd.

CP03 Change of name, title or address