CN111409073A - 一种高智能机器人的摔倒自恢复方法及系统 - Google Patents

Abstract

本发明提供一种高智能机器人的摔倒自恢复方法及系统，涉及机器人控制技术领域，包括：在高智能机器人处于摔倒状态时获取其当前摔倒姿势，提取与当前摔倒姿势匹配度最高的标准摔倒姿势；控制各驱动电机带动驱动关节动作，以控制高智能机器人由当前摔倒姿势恢复成标准摔倒姿势；根据标准摔倒姿势对应的恢复动作序列，控制各驱动关节根据恢复动作序列动作，以控制高智能机器人由标准摔倒姿势恢复成标准恢复姿势；获取高智能机器人在摔倒前一运动时刻对应的实时运动轨迹，表征高智能机器人的摔倒前姿势，并控制高智能机器人由标准恢复姿势恢复成摔倒前姿势。有效提升高智能机器人的摔倒检测准确性的同时降低摔倒误报率，且能够摔倒自恢复。

Description

一种高智能机器人的摔倒自恢复方法及系统

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种高智能机器人的摔倒自恢复方法及系统。

背景技术

高智能机器人具有类似人类的外形特征和运动能力，通常被用来在不改变现有的人工环境的前提下辅助或代替人进行各种工作，能够在非特定环境下行走，以完成给定的任务动作，由于行走环境是未知的，有些情况下难免会出现摔倒问题，进而无法正确执行后续任务动作，若不能及时检测到机器人的摔倒状态，且对机器人进行摔倒自恢复的实时调整，会影响机器人的任务执行效率，且容易造成站立机器人的部件损坏。因此亟需一种能够及时检测机器人的摔倒状态并能够从摔倒状态自恢复的方法及系统。

发明内容

针对现有技术中存在的问题，本发明提供一种高智能机器人的摔倒自恢复方法，所述高智能机器人的每个行走部件具有至少一驱动关节，每个所述驱动关节分别通过一驱动电机连接所述高智能机器人的控制器；所述高智能机器人还具有一惯性测量单元，所述惯性测量单元连接所述控制器；

所述控制器中预先保存有若干摔倒恢复数据集合，每个所述摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据所述标准摔倒姿势和预设的标准恢复姿势生成的恢复动作序列；

则所述摔倒自恢复方法具体包括：

步骤S1，所述控制器通过所述惯性测量单元获取所述高智能机器人的实时运动轨迹且按照所述高智能机器人的运动时刻保存所述实时运动轨迹，并在所述实时运动轨迹表示所述高智能机器人处于摔倒状态时输出相应的告警提示；

步骤S2，所述控制器根据所述告警提示通过所述惯性测量单元获取所述高智能机器人的当前摔倒姿势，分别计算所述当前摔倒姿势与各所述摔倒恢复数据集合中的所述标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取所述匹配度序列中所述匹配度最高的所述摔倒恢复数据集合；

步骤S3，所述控制器根据所述当前摔倒姿势和提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势，控制各所述驱动电机带动所述驱动关节动作，以控制所述高智能机器人由所述当前摔倒姿势恢复成所述标准摔倒姿势；

步骤S4，所述控制器根据提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势对应的所述恢复动作序列，控制各所述驱动电机带动所述驱动关节根据所述恢复动作序列进行相应动作，以控制所述高智能机器人由所述标准摔倒姿势恢复成所述标准恢复姿势；

步骤S5，所述控制器获取所述高智能机器人在摔倒前一所述运动时刻对应的所述实时运动轨迹，以表征所述高智能机器人的摔倒前姿势，并控制所述高智能机器人由所述标准恢复姿势恢复成所述摔倒前姿势。

优选的，所述高智能机器人为站立机器人；

则所述步骤S1具体包括：

步骤S11a，所述控制器获取各所述驱动关节的实时关节位置数据，并根据各所述实时关节位置数据处理得到所述站立机器人的所述行走部件的实时运动轨迹；

步骤S12a，所述控制器计算所述实时运动轨迹与预设的标准运动轨迹之间的偏差值，并将所述偏差值与预设的偏差阈值进行比较：

若所述偏差值小于所述偏差阈值，则返回所述步骤S11a；

若所述偏差值不小于所述偏差阈值，则转向步骤S13a；

步骤S13a，所述控制器通过所述惯性测量单元获取所述站立机器人的第一实时俯仰角，并将所述第一实时俯仰角与预设的第一俯仰角阈值进行比较：

若所述第一实时俯仰角不大于所述第一俯仰角阈值，则转向步骤S14a；

若所述第一实时俯仰角大于所述第一俯仰角阈值，则转向步骤S15a；

步骤S14a，所述控制器输出表征所述站立机器人出现运动轨迹偏差的轨迹偏差提示，随后返回所述步骤S11a；

步骤S15a，所述控制器输出表征所述站立机器人处于摔倒状态的告警提示。

优选的，所述步骤S13a具体包括：

步骤S131a，所述控制器通过所述惯性测量单元获取所述站立机器人的第一实时俯仰角，所述第一实时俯仰角包括俯仰角值以及所述俯仰角值对应的数值符号；

步骤S132a，所述控制器将所述俯仰角值与预设的俯仰角阈值进行比较：

若所述俯仰角值不大于所述俯仰角阈值，则返回所述步骤S14a；

若所述俯仰角值大于所述俯仰角阈值，则转向步骤S133a；

步骤S133a，所述控制器根据所述俯仰角值对应的所述数值符号进行判断：

若所述数值符号为正，则转向步骤S134a；

若所述数值符号为负，则转向步骤S135a；

步骤S134a，所述控制器生成表征所述站立机器人为前向摔倒的告警提示，随后转向步骤S15a；

步骤S135a，所述控制器生成表征所述站立机器人为后向摔倒的告警提示，随后转向步骤S15a。

优选的，每个所述驱动关节连接一离合器，且所述离合器通过所述驱动电机连接所述控制器；

则所述步骤S132a中，若所述俯仰角值不大于所述俯仰角阈值，还包括：

步骤A1，所述控制器获取所述驱动电机的输出电流，并根据所述输出电流计算得到所述离合器的第一实时扭矩；

步骤A2，所述控制器将所述第一实时扭矩与预设的第一扭矩阈值进行比较：

若所述第一实时扭矩大于所述第一扭矩阈值，则转向步骤A3；

若所述第一实时扭矩不大于所述第一扭矩阈值，则转向所述步骤S133a；

步骤A3，所述控制器将所述第一实时扭矩与预设的第二扭矩阈值进行比较：

若所述第一实时扭矩小于所述第二扭矩阈值，则转向所述步骤S14a；

若所述第一实时扭矩不小于所述第二扭矩阈值，则转向所述步骤S133a；

所述第一扭矩阈值小于所述第二扭矩阈值。

优选的，所述高智能机器人为爬行机器人；

则所述步骤S1具体包括：

步骤S11b，针对每个所述驱动关节，获取所述驱动关节的实时关节位置数据并输出；

步骤S12b，所述控制器分别计算各所述实时关节位置数据与对应连接所述驱动电机的预设电机位置数据之间的位置差值，并分别将各所述位置差值与预设的第一差值阈值进行比较：

若存在所述位置差值大于所述第一差值阈值，则转向步骤S13b；

若不存在所述位置差值大于所述第一差值阈值，则返回所述步骤S11b；

步骤S13b，所述控制器控制所述驱动电机停止动作并输出表征所述爬行机器人处于摔倒状态的告警提示。

优选的，所述步骤S12b中，若不存在所述位置差值大于所述第一差值阈值，还包括：

步骤B1，所述控制器分别将所述位置差值与预设的第二差值阈值进行比较：

若存在所述位置差值大于所述第二差值阈值，则转向步骤B2；

若不存在所述位置差值大于所述第二差值阈值，则返回所述步骤S11b；

所述第二差值阈值小于所述第一差值阈值；

步骤B2，所述控制器通过所述惯性测量单元获取所述爬行机器人的第二实时俯仰角，并将所述第二实时俯仰角与预设的第二俯仰角阈值进行比较：

若所述第二实时俯仰角小于所述第二俯仰角阈值，则转向所述步骤S13b；

若所述第二实时俯仰角不小于所述第二俯仰角阈值，则返回所述步骤S11b。

优选的，每个所述驱动关节连接一离合器，且所述离合器通过所述驱动电机连接所述控制器；

则所述步骤B2中，若所述第二实时俯仰角不小于所述第二俯仰角阈值，还包括：

步骤C1，所述控制器获取所述驱动电机的输出电流，并根据所述输出电流计算得到所述离合器的第二实时扭矩；

步骤C2，所述控制器将所述第二实时扭矩与预设的第三扭矩阈值进行比较：

若所述第二实时扭矩大于所述第三扭矩阈值，则转向步骤C3；

若所述第二实时扭矩不大于所述第三扭矩阈值，则转向所述步骤S13b；

步骤C3，所述控制器将所述第二实时扭矩与预设的第四扭矩阈值进行比较：

若所述实时扭矩小于所述第四扭矩阈值，则返回所述步骤S11b；

若所述实时扭矩不小于所述第四扭矩阈值，则转向所述步骤S13b；

所述第三扭矩阈值小于所述第四扭矩阈值。

优选的，所述驱动关节内部设有编码器，通过所述编码器获取对应的所述驱动关节的实时关节位置数据并输出至所述控制器。

一种高智能机器人的摔倒自恢复系统，应用以上任意一项所述的高智能机器人的摔倒自恢复方法，所述摔倒自恢复系统具体包括：

控制器，所述控制器包括：

第一存储单元，用于在所述高智能机器人的运动过程中，按照所述高智能机器人的运动时刻对处理得到的所述实时运动轨迹进行保存；

第二存储单元，用于保存预先生成的若干摔倒恢复数据集合，每个所述摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据所述标准摔倒姿势和预设的标准站立姿势生成的恢复动作序列；

姿势匹配单元，连接所述第二存储单元，用于根据所述告警提示通过所述惯性测量单元获取所述高智能机器人的当前摔倒姿势，分别计算所述当前摔倒姿势与各所述摔倒恢复数据集合中的所述标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取所述匹配度序列中所述匹配度最高的所述摔倒恢复数据集合并输出；

第一恢复单元，连接所述姿势匹配单元，用于根据所述当前摔倒姿势和提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势，控制各所述驱动关节动作，以控制所述高智能机器人由所述当前摔倒姿势恢复成所述标准摔倒姿势；

第二恢复单元，分别连接所述姿势匹配单元和所述第一恢复单元，用于根据提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势对应的所述恢复动作序列，控制各所述驱动关节根据所述恢复动作序列进行相应动作，以控制所述高智能机器人由所述标准摔倒姿势恢复成所述标准站立姿势；

第三恢复单元，分别连接所述第一存储单元和所述第二恢复单元，用于获取所述高智能机器人在摔倒前一所述运动时刻对应的所述实时运动轨迹，以表征所述高智能机器人的摔倒前姿势，并控制所述高智能机器人由所述标准站立姿势恢复成所述摔倒前姿势。

上述技术方案具有如下优点或有益效果：

1)能够及时检测高智能机器人的摔倒状态，检测准确率高，有效减少摔倒误报警；

2)在高智能机器人的行走部件的运动轨迹出现偏差时，能够及时获取偏差状态，方便高智能机器人的操纵者对站立机器人的控制策略进行及时调整，有效提升高智能机器人的控制精准度；

3)在检测到高智能机器人处于摔倒状态时，及时控制其驱动关节停止动作，以避免由于摔倒引起的驱动关节空转或打滑；

4)高智能机器人在摔倒后能够快速自恢复成摔倒前姿势，有效提升高智能机器人的任务执行效率以及智能化程度。

附图说明

图1为本发明的较佳的实施例中，一种高智能机器人的摔倒自恢复方法的流程示意图；

图2为本发明的较佳的实施例中，站立机器人的摔倒检测方法的流程示意图；

图3为本发明的较佳的实施例中，站立机器人根据第一实时俯仰角进行摔倒检测的方法的流程示意图；

图4为本发明的较佳的实施例中，站立机器人根据离合器的第一实时扭矩进行摔倒检测的方法的流程示意图；

图5为本发明的较佳的实施例中，爬行机器人的摔倒检测方法的流程示意图；

图6为本发明的较佳的实施例中，爬行机器人根据第二实时俯仰角进行摔倒检测的方法的流程示意图；

图7为本发明的较佳的实施例中，爬行机器人根据离合器的第二实时扭矩进行摔倒检测的方法的流程示意图；

图8为本发明的较佳的实施例中，一种高智能机器人的摔倒自恢复系统的结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本发明并不限定于该实施方式，只要符合本发明的主旨，则其他实施方式也可以属于本发明的范畴。

本发明的较佳的实施例中，基于现有技术中存在的上述问题，现提供一种高智能机器人的摔倒自恢复方法，如图8所示，高智能机器人的每个行走部件具有至少一驱动关节1，每个驱动关节1分别通过一驱动电机2连接高智能机器人的控制器3；高智能机器人还具有一惯性测量单元4，惯性测量单元4连接控制器3；

控制器3中预先保存有若干摔倒恢复数据集合，每个摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据标准摔倒姿势和预设的标准恢复姿势生成的恢复动作序列；

如图1所示，则摔倒自恢复方法具体包括：

步骤S1，控制器通过惯性测量单元获取高智能机器人的实时运动轨迹且按照高智能机器人的运动时刻保存实时运动轨迹，并在实时运动轨迹表示高智能机器人处于摔倒状态时输出相应的告警提示；

步骤S2，控制器根据告警提示通过惯性测量单元获取高智能机器人的当前摔倒姿势，分别计算当前摔倒姿势与各摔倒恢复数据集合中的标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取匹配度序列中匹配度最高的摔倒恢复数据集合；

步骤S3，控制器根据当前摔倒姿势和提取得到的摔倒恢复数据集合中的标准摔倒姿势，控制各驱动电机带动驱动关节动作，以控制高智能机器人由当前摔倒姿势恢复成标准摔倒姿势；

步骤S4，控制器根据提取得到的摔倒恢复数据集合中的标准摔倒姿势对应的恢复动作序列，控制各驱动电机带动驱动关节根据恢复动作序列进行相应动作，以控制高智能机器人由标准摔倒姿势恢复成标准恢复姿势；

步骤S5，控制器获取高智能机器人在摔倒前一运动时刻对应的实时运动轨迹，以表征高智能机器人的摔倒前姿势，并控制高智能机器人由标准恢复姿势恢复成摔倒前姿势。

具体地，本实施例中，高智能机器人在执行任务的过程中，为了不影响任务执行效率，优选在检测到高智能机器人处于摔倒姿势时，能够进一步控制高智能机器人快速恢复到摔倒前的姿势，无需人工参与过程，从而继续执行任务，进一步提升高智能机器人的智能化程度。

进一步具体地，为了使得高智能机器人能够快速恢复至摔倒前姿势，首先需要获取摔倒前姿势，本实施例中，优选在高智能机器人的运动过程中获取其实时运动轨迹，该实时运动轨迹优选关联于高智能机器人的运动时刻。以接收到告警提示作为分界，优选将收到告警提示前一运动时刻的实时运动轨迹作为摔倒前姿势。

在确定了摔倒前姿势后，需要控制高智能机器人快速由当前摔倒姿势恢复至摔倒前姿势。由于高智能机器人的当前摔倒姿势是随机的，若控制高智能机器人由当前摔倒姿势直接恢复至摔倒前姿势，需要经过复杂的运算控制过程，且效率比较低。因此，本实施例中，在控制器中预先生成若干标准摔倒姿势，以及由对应的标准摔倒姿势最快恢复至预设的标准恢复姿势的恢复动作序列。同时将高智能机器人的摔倒恢复过程分为三个阶段：第一阶段是先控制高智能机器人从当前摔倒姿势恢复成与当前摔倒姿势最相近的标准摔倒姿势，此时由于标准摔倒姿势与当前摔倒姿势最为相近，高智能机器人能够以最少的动作过程恢复到标准摔倒姿势；第二阶段是控制高智能机器人由标准摔倒姿势快速恢复成标准恢复姿势，由于由标准摔倒姿势恢复成标准恢复姿势需要经过的恢复动作序列是预先生成的，高智能机器人只需执行即可，有效提升恢复速率；第三阶段是控制高智能机器人由标准恢复姿势恢复成摔倒前姿势，实现高智能机器人的摔倒自恢复。

进一步具体地，本发明的高智能机器人为站立机器人时，上述标准恢复姿势优选为标准站立姿势，本发明的高智能机器人为爬行机器人时，上述标准恢复姿势优选为标准爬行姿势。

本实施例中，惯性测量单元4(Inertial measurement unit，简称IMU)优选设置于高智能机器人的躯干部位，惯性测量单元4包括三轴加速度计、三轴陀螺仪以及三轴磁力计，其中，三轴加速度计和三轴陀螺仪能够测量高智能机器人相对于重力方向的姿态，而三轴磁力计能够提供相对于重力方向和地球磁场的方向的完整测量。具体地，三轴加速度计是一种惯性传感器，测量时既不受运动限制，也不受任何特定环境或位置的限制，能够精确测量高智能机器人的俯仰角和滚转角，且没有累积误差；三轴陀螺仪能够检测到高智能机器人的角速度；三轴磁力计能够提供高智能机器人在X轴、Y轴和Z轴所承受磁场的数据，以提供磁北极相关的航向角，高智能机器人在动作过程中能够利用该信息进行地理方位的侦测。

进一步地，本发明中，通过上述三轴加速度计测量得到的高智能机器人的俯仰角，能够进行高智能机器人是否处于摔倒状态的判定，并在高智能机器人处于摔倒状态时，控制器能够通过高智能机器人当前摔倒状态时的俯仰角、滚转角和角速度处理得到高智能机器人的当前摔倒姿势，进而能够控制高智能机器人由当前摔倒姿势进行自恢复。

本发明的较佳的实施例中，高智能机器人为站立机器人；

如图2所示，则步骤S1具体包括：

步骤S11a，控制器获取各驱动关节的实时关节位置数据，并根据各实时关节位置数据处理得到站立机器人的行走部件的实时运动轨迹；

步骤S12a，控制器计算实时运动轨迹与预设的标准运动轨迹之间的偏差值，并将偏差值与预设的偏差阈值进行比较：

若偏差值小于偏差阈值，则返回步骤S11a；

若偏差值不小于偏差阈值，则转向步骤S13a；

步骤S13a，控制器通过惯性测量单元获取站立机器人的第一实时俯仰角，并将第一实时俯仰角与预设的第一俯仰角阈值进行比较：

若第一实时俯仰角不大于第一俯仰角阈值，则转向步骤S14a；

若第一实时俯仰角大于第一俯仰角阈值，则转向步骤S15a；

步骤S14a，控制器输出表征站立机器人出现运动轨迹偏差的轨迹偏差提示，随后返回步骤S11a；

步骤S15a，控制器输出表征站立机器人处于摔倒状态的告警提示。

具体地，本实施例中，本发明的站立机器人优选为双足机器人，通过实时检测站立机器人的行走部件的运动轨迹，并在行走部件的实时运动轨迹与标准运动轨迹之间的偏差值较大时，进一步通过检测站立机器人的实时俯仰角，最终确定站立机器人是否处于摔倒状态，有效减少单一的摔倒约束条件导致的摔倒误报率高的问题。

进一步具体地，站立机器人的行走部件的动作过程中，由于实时环境的不同，行走部件的运动轨迹与站立机器人的操纵者的期望运行轨迹，即上述标准运动轨迹会出现偏差，若偏差在正常范围内，则判定机器人不处于摔倒状态，若偏差不在正常范围内，则站立机器人有可能处于摔倒状态。本发明通过预设的偏差阈值来约束上述正常范围，优选偏差值小于偏差阈值为正常范围，偏差值不小于偏差阈值为可能出现摔倒状态的范围。

在偏差值不小于偏差阈值时，进一步通过惯性测量单元4检测站立机器人当前的第一实时俯仰角。站立机器人在不处于摔倒状态时，第一实时俯仰角应当为约0度，若第一实时俯仰角过大，则判定为摔倒状态，考虑到行走机器人的运动环境以及操作任务的不同，第一实时俯仰角有可能会因为不同的操作任务出现波动，本发明通过预设的第一俯仰角阈值来约束第一实时俯仰角的正常波动范围。第一实时俯仰角不大于第一俯仰角阈值时，则判定站立机器人不处于摔倒状态，即上述偏差值是由于外部环境等其他因素导致，此时，站立机器人的控制器仅输出表征站立机器人出现运动轨迹偏差的轨迹偏差提示，供操纵者进行参考，是否需要相应调整站立机器人的控制策略。第一实时俯仰角大于第一俯仰角阈值时，则判定机器人处于摔倒状态，此时，站立机器人的控制器输出表征站立机器人处于摔倒状态的告警提示，以供站立机器人的进行摔倒自恢复。

本发明的较佳的实施例中，如图3所示，步骤S13a具体包括：

步骤S131a，控制器通过惯性测量单元获取站立机器人的第一实时俯仰角，第一实时俯仰角包括俯仰角值以及俯仰角值对应的数值符号；

步骤S132a，控制器将俯仰角值与预设的俯仰角阈值进行比较：

若俯仰角值不大于俯仰角阈值，则返回步骤S14a；

若俯仰角值大于俯仰角阈值，则转向步骤S133a；

步骤S133a，控制器根据俯仰角值对应的数值符号进行判断：

若数值符号为正，则转向步骤S134a；

若数值符号为负，则转向步骤S135a；

步骤S134a，控制器生成表征站立机器人为前向摔倒的告警提示，随后转向步骤S15a；

步骤S135a，控制器生成表征站立机器人为后向摔倒的告警提示，随后转向步骤S15a。

具体地，本实施例中，在通过站立机器人的第一实时俯仰角进行摔倒检测判定时，可以进一步通过第一实时俯仰角的正负号更为精准的判断站立机器人是向前摔倒还是向后摔倒，以供后续进一步研究使用。优选为第一实时俯仰角的俯仰角值为正值时，判定站立机器人为前向摔倒，第一实时俯仰角的俯仰角值为负值时，判定站立机器人为后向摔倒。

本发明的较佳的实施例中，每个驱动关节1连接一离合器5，且离合器5通过驱动电机2连接控制器3；

如图4所示，则步骤S132a中，若俯仰角值不大于俯仰角阈值，还包括：

步骤A1，控制器获取驱动电机的输出电流，并根据输出电流计算得到离合器的第一实时扭矩；

步骤A2，控制器将第一实时扭矩与预设的第一扭矩阈值进行比较：

若第一实时扭矩大于第一扭矩阈值，则转向步骤A3；

若第一实时扭矩不大于第一扭矩阈值，则转向步骤S133a；

步骤A3，控制器将第一实时扭矩与预设的第二扭矩阈值进行比较：

若第一实时扭矩小于第二扭矩阈值，则转向步骤S14a；

若第一实时扭矩不小于第二扭矩阈值，则转向步骤S133a；

第一扭矩阈值小于第二扭矩阈值。

具体地，本实施例中，若俯仰角值不大于俯仰角阈值时，为避免由于外界环境的支撑，使得站立机器人的第一实时俯仰角在正常波动范围内，造成站立机器人的摔倒检测的遗漏，进一步通过检测驱动关节1的离合器5的第一实时扭矩，通过检测第一实时扭矩是否在正常扭矩范围内，进一步判定站立机器人是否处于摔倒状态。站立机器人的正常工作状态时，离合器5的第一实时扭矩应当在正常扭矩范围内，若站立机器人摔倒，则离合器5有可能出现打滑，从而使得第一实时扭矩不在正常扭矩范围内。因此，可能通过预设一正常扭矩范围，即上述第一扭矩阈值和第二扭矩阈值限定的扭矩区域范围，进而判定站立机器人的摔倒状态。

进一步具体地，根据以下公式计算得到第一实时扭矩：

其中，

T用于表示第一实时扭矩；

I用于表示驱动电机的输出电流；

C用于表示驱动电机的电机常数；

R用于表示驱动电机的齿轮比。

本发明的较佳的实施例中，高智能机器人为爬行机器人；

如图5所示，则步骤S1具体包括：

步骤S11b，针对每个驱动关节，获取驱动关节的实时关节位置数据并输出；

步骤S12b，控制器分别计算各实时关节位置数据与对应连接驱动电机的预设电机位置数据之间的位置差值，并分别将各位置差值与预设的第一差值阈值进行比较：

若存在位置差值大于第一差值阈值，则转向步骤S13b；

若不存在位置差值大于第一差值阈值，则返回步骤S11b；

步骤S13b，控制器控制驱动电机停止动作并输出表征爬行机器人处于摔倒状态的告警提示。

具体地，本实施例中，爬行机器人正常行走及动作过程中，驱动电机2的电机位置数据与驱动关节1的实时关节位置数据相一致。因此，由于驱动电机2的电机位置数据是对应不变的，优选将电机位置数据预先存储在控制器中，控制器3实时获取每组驱动装置的驱动关节1的实时关节位置数据，并在检测到存在至少一个驱动关节1的实时关节位置数据与对应连接的驱动电机2的预设电机位置数据之间的差值大于上述第一差值阈值时，则判定爬行机器人当前处于摔倒状态，除输出相应的告警提示外，控制器3还控制驱动关节1停止运行，即爬行机器人停止当前动作，以避免驱动电机2空转或与驱动电机2连接的离合器5打滑。

上述第一差值阈值优选采用相对较大的值，避免由于实时关节位置数据与预设电机位置数据之间较小的差值引起的误报警。

本发明的较佳的实施例中，步骤S12b中，若不存在位置差值大于第一差值阈值，如图6所示，还包括：

步骤B1，控制器分别将位置差值与预设的第二差值阈值进行比较：

若存在位置差值大于第二差值阈值，则转向步骤B2；

若不存在位置差值大于第二差值阈值，则返回步骤S11b；

第二差值阈值小于第一差值阈值；

步骤B2，控制器通过惯性测量单元获取爬行机器人的第二实时俯仰角，并将第二实时俯仰角与预设的第二俯仰角阈值进行比较：

若第二实时俯仰角小于第二俯仰角阈值，则转向步骤S13b；

若第二实时俯仰角不小于第二俯仰角阈值，则返回步骤S11b。

具体地，本实施例中，由于上述第一差值阈值优选取较大的值，在避免误报警的同时，有可能会出现摔倒检测的遗漏，即由于上述第一差值阈值选取过大，导致爬行机器人出现摔倒时部分情况检测不到。优选设置小于上述第一差值阈值的一第二差值阈值，在各实时关节位置数据与对应的各预设电机位置数据的差值均小于第一差值阈值时，继续将上述差值与第二差值阈值进行比较，在存在至少一个驱动关节1的实时关节位置数据与对应连接的驱动电机2的预设电机位置数据之间的差值大于上述第二差值阈值时，则判定爬行机器人当前处于摔倒状态。同样的，上述第二差值阈值的选取能够检测出部分被遗漏的摔倒状态，但考虑误报警造成的频繁摔倒告警提示带来的操作不便利性，第二差值阈值也不能取较小的值，因此，仍可能出现爬行机器人的部分情况下的摔倒状态无法检测到。

本实施例中，爬行机器人内部设置有与控制器3连接的一惯性测量单元4，通过上述惯性测量单元4能够检测到爬行机器人的当前姿态，上述当前姿态优选通过爬行机器人的第二实时俯仰角进行表征。由于爬行机器人在正常行走状态时，第二实时俯仰角约等于负90度，爬行机器人在摔倒状态时，通常处于一个较小的俯仰角，因此，通过设定一个第二俯仰角阈值，并在爬行机器人的第二实时俯仰角小于该第二俯仰角阈值时判定爬行机器人当前处于摔倒状态。上述第二俯仰角阈值优选为负50度。

本发明的较佳的实施例中，每个驱动关节1连接一离合器5，且离合器5通过驱动电机2连接控制器3；

则步骤B2中，若第二实时俯仰角不小于第二俯仰角阈值，如图7所示，还包括：

步骤C1，控制器获取驱动电机的输出电流，并根据输出电流计算得到离合器的第二实时扭矩；

步骤C2，控制器将第二实时扭矩与预设的第三扭矩阈值进行比较：

若第二实时扭矩大于第三扭矩阈值，则转向步骤C3；

若第二实时扭矩不大于第三扭矩阈值，则转向步骤S13b；

步骤C3，控制器将第二实时扭矩与预设的第四扭矩阈值进行比较：

若实时扭矩小于第四扭矩阈值，则返回步骤S11b；

若实时扭矩不小于第四扭矩阈值，则转向步骤S13b；

第三扭矩阈值小于第四扭矩阈值。

具体地，本实施例中，为避免上述第二俯仰角阈值的设置，遗漏部分特殊情况下的爬行机器人的摔倒状态，更进一步优选，在上述第二实时俯仰角不小于第二俯仰角阈值时，通过检测连接驱动电机2和驱动关节1的离合器5的打滑情况，进一步判定爬行机器人是否处于摔倒状态。爬行机器人正常行走状态时，上述离合器5的第二实时扭矩应处于一正常区间范围，即上述第三扭矩阈值和上述第四扭矩阈值形成的扭矩区间范围。在离合器5不处于上述扭矩区间范围时，则可以判定爬行机器人当前处于摔倒状态。

进一步具体地，根据以下公式计算得到第二实时扭矩：

其中，

T用于表示第二实时扭矩；

I用于表示驱动电机的输出电流；

C用于表示驱动电机的电机常数；

R用于表示驱动电机的齿轮比。

本发明的较佳的实施例中，驱动关节1内部设有编码器6，通过编码器6获取对应的驱动关节1的实时关节位置数据并输出至控制器3。

具体地，本实施例中，上述编码器6包括但不限于增量编码器和绝对编码器。

一种高智能机器人的摔倒自恢复系统，应用以上任意一项的高智能机器人的摔倒自恢复方法，如图8所示，摔倒自恢复系统具体包括：

控制器3，所述控制器3包括：

第一存储单元31，用于在高智能机器人的运动过程中，按照高智能机器人的运动时刻对处理得到的实时运动轨迹进行保存；

第二存储单元32，用于保存预先生成的若干摔倒恢复数据集合，每个摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据标准摔倒姿势和预设的标准站立姿势生成的恢复动作序列；

姿势匹配单元33，连接第二存储单元32，用于根据告警提示通过惯性测量单元获取高智能机器人的当前摔倒姿势，分别计算当前摔倒姿势与各摔倒恢复数据集合中的标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取匹配度序列中匹配度最高的摔倒恢复数据集合并输出；

第一恢复单元34，连接姿势匹配单元33，用于根据当前摔倒姿势和提取得到的摔倒恢复数据集合中的标准摔倒姿势，控制各驱动关节动作，以控制高智能机器人由当前摔倒姿势恢复成标准摔倒姿势；

第二恢复单元35，分别连接姿势匹配单元33和第一恢复单元34，用于根据提取得到的摔倒恢复数据集合中的标准摔倒姿势对应的恢复动作序列，控制各驱动关节根据恢复动作序列进行相应动作，以控制高智能机器人由标准摔倒姿势恢复成标准站立姿势；

第三恢复单元36，分别连接第一存储单元31和第二恢复单元35，用于获取高智能机器人在摔倒前一运动时刻对应的实时运动轨迹，以表征高智能机器人的摔倒前姿势，并控制高智能机器人由标准站立姿势恢复成摔倒前姿势。

以上所述仅为本发明较佳的实施例，并非因此限制本发明的实施方式及保护范围，对于本领域技术人员而言，应当能够意识到凡运用本说明书及图示内容所作出的等同替换和显而易见的变化所得到的方案，均应当包含在本发明的保护范围内。

Claims (9)

1.一种高智能机器人的摔倒自恢复方法，其特征在于，所述高智能机器人的每个行走部件具有至少一驱动关节，每个所述驱动关节分别通过一驱动电机连接所述高智能机器人的控制器；所述高智能机器人还具有一惯性测量单元，所述惯性测量单元连接所述控制器；

所述控制器中预先保存有若干摔倒恢复数据集合，每个所述摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据所述标准摔倒姿势和预设的标准恢复姿势生成的恢复动作序列；

则所述摔倒自恢复方法具体包括：

步骤S1，所述控制器通过所述惯性测量单元获取所述高智能机器人的实时运动轨迹且按照所述高智能机器人的运动时刻保存所述实时运动轨迹，并在所述实时运动轨迹表示所述高智能机器人处于摔倒状态时输出相应的告警提示；

步骤S2，所述控制器根据所述告警提示通过所述惯性测量单元获取所述高智能机器人的当前摔倒姿势，分别计算所述当前摔倒姿势与各所述摔倒恢复数据集合中的所述标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取所述匹配度序列中所述匹配度最高的所述摔倒恢复数据集合；

步骤S3，所述控制器根据所述当前摔倒姿势和提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势，控制各所述驱动电机带动所述驱动关节动作，以控制所述高智能机器人由所述当前摔倒姿势恢复成所述标准摔倒姿势；

步骤S4，所述控制器根据提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势对应的所述恢复动作序列，控制各所述驱动电机带动所述驱动关节根据所述恢复动作序列进行相应动作，以控制所述高智能机器人由所述标准摔倒姿势恢复成所述标准恢复姿势；

步骤S5，所述控制器获取所述高智能机器人在摔倒前一所述运动时刻对应的所述实时运动轨迹，以表征所述高智能机器人的摔倒前姿势，并控制所述高智能机器人由所述标准恢复姿势恢复成所述摔倒前姿势。

2.根据权利要求1所述的高智能机器人的摔倒自恢复方法，其特征在于，所述高智能机器人为站立机器人；

则所述步骤S1具体包括：

步骤S11a，所述控制器获取各所述驱动关节的实时关节位置数据，并根据各所述实时关节位置数据处理得到所述站立机器人的所述行走部件的实时运动轨迹；

步骤S12a，所述控制器计算所述实时运动轨迹与预设的标准运动轨迹之间的偏差值，并将所述偏差值与预设的偏差阈值进行比较：

若所述偏差值小于所述偏差阈值，则返回所述步骤S11a；

若所述偏差值不小于所述偏差阈值，则转向步骤S13a；

步骤S13a，所述控制器通过所述惯性测量单元获取所述站立机器人的第一实时俯仰角，并将所述第一实时俯仰角与预设的第一俯仰角阈值进行比较：

若所述第一实时俯仰角不大于所述第一俯仰角阈值，则转向步骤S14a；

若所述第一实时俯仰角大于所述第一俯仰角阈值，则转向步骤S15a；

步骤S14a，所述控制器输出表征所述站立机器人出现运动轨迹偏差的轨迹偏差提示，随后返回所述步骤S11a；

步骤S15a，所述控制器输出表征所述站立机器人处于摔倒状态的告警提示。

3.根据权利要求2所述的高智能机器人的摔倒自恢复方法，其特征在于，所述步骤S13a具体包括：

步骤S131a，所述控制器通过所述惯性测量单元获取所述站立机器人的第一实时俯仰角，所述第一实时俯仰角包括俯仰角值以及所述俯仰角值对应的数值符号；

步骤S132a，所述控制器将所述俯仰角值与预设的俯仰角阈值进行比较：

若所述俯仰角值不大于所述俯仰角阈值，则返回所述步骤S14a；

若所述俯仰角值大于所述俯仰角阈值，则转向步骤S133a；

步骤S133a，所述控制器根据所述俯仰角值对应的所述数值符号进行判断：

若所述数值符号为正，则转向步骤S134a；

若所述数值符号为负，则转向步骤S135a；

步骤S134a，所述控制器生成表征所述站立机器人为前向摔倒的告警提示，随后转向步骤S15a；

步骤S135a，所述控制器生成表征所述站立机器人为后向摔倒的告警提示，随后转向步骤S15a。

4.根据权利要求3所述的高智能机器人的摔倒自恢复方法，其特征在于，每个所述驱动关节连接一离合器，且所述离合器通过所述驱动电机连接所述控制器；

则所述步骤S132a中，若所述俯仰角值不大于所述俯仰角阈值，还包括：

步骤A1，所述控制器获取所述驱动电机的输出电流，并根据所述输出电流计算得到所述离合器的第一实时扭矩；

步骤A2，所述控制器将所述第一实时扭矩与预设的第一扭矩阈值进行比较：

若所述第一实时扭矩大于所述第一扭矩阈值，则转向步骤A3；

若所述第一实时扭矩不大于所述第一扭矩阈值，则转向所述步骤S133a；

步骤A3，所述控制器将所述第一实时扭矩与预设的第二扭矩阈值进行比较：

若所述第一实时扭矩小于所述第二扭矩阈值，则转向所述步骤S14a；

若所述第一实时扭矩不小于所述第二扭矩阈值，则转向所述步骤S133a；

所述第一扭矩阈值小于所述第二扭矩阈值。

5.根据权利要求1所述的高智能机器人的摔倒自恢复方法，其特征在于，所述高智能机器人为爬行机器人；

则所述步骤S1具体包括：

步骤S11b，针对每个所述驱动关节，获取所述驱动关节的实时关节位置数据并输出；

步骤S12b，所述控制器分别计算各所述实时关节位置数据与对应连接所述驱动电机的预设电机位置数据之间的位置差值，并分别将各所述位置差值与预设的第一差值阈值进行比较：

若存在所述位置差值大于所述第一差值阈值，则转向步骤S13b；

若不存在所述位置差值大于所述第一差值阈值，则返回所述步骤S11b；

步骤S13b，所述控制器控制所述驱动电机停止动作并输出表征所述爬行机器人处于摔倒状态的告警提示。

6.根据权利要求5所述的高智能机器人的摔倒自恢复方法，其特征在于，所述步骤S12b中，若不存在所述位置差值大于所述第一差值阈值，还包括：

步骤B1，所述控制器分别将所述位置差值与预设的第二差值阈值进行比较：

若存在所述位置差值大于所述第二差值阈值，则转向步骤B2；

若不存在所述位置差值大于所述第二差值阈值，则返回所述步骤S11b；

所述第二差值阈值小于所述第一差值阈值；

步骤B2，所述控制器通过所述惯性测量单元获取所述爬行机器人的第二实时俯仰角，并将所述第二实时俯仰角与预设的第二俯仰角阈值进行比较：

若所述第二实时俯仰角小于所述第二俯仰角阈值，则转向所述步骤S13b；

若所述第二实时俯仰角不小于所述第二俯仰角阈值，则返回所述步骤S11b。

7.根据权利要求6所述的高智能机器人的摔倒自恢复方法，其特征在于，每个所述驱动关节连接一离合器，且所述离合器通过所述驱动电机连接所述控制器；

则所述步骤B2中，若所述第二实时俯仰角不小于所述第二俯仰角阈值，还包括：

步骤C1，所述控制器获取所述驱动电机的输出电流，并根据所述输出电流计算得到所述离合器的第二实时扭矩；

步骤C2，所述控制器将所述第二实时扭矩与预设的第三扭矩阈值进行比较：

若所述第二实时扭矩大于所述第三扭矩阈值，则转向步骤C3；

若所述第二实时扭矩不大于所述第三扭矩阈值，则转向所述步骤S13b；

步骤C3，所述控制器将所述第二实时扭矩与预设的第四扭矩阈值进行比较：

若所述实时扭矩小于所述第四扭矩阈值，则返回所述步骤S11b；

若所述实时扭矩不小于所述第四扭矩阈值，则转向所述步骤S13b；

所述第三扭矩阈值小于所述第四扭矩阈值。

8.根据权利要求2或5所述的高智能机器人的摔倒自恢复方法，其特征在于，所述驱动关节内部设有编码器，通过所述编码器获取对应的所述驱动关节的实时关节位置数据并输出至所述控制器。

9.一种高智能机器人的摔倒自恢复系统，其特征在于，应用如权利要求1-8中任意一项所述的高智能机器人的摔倒自恢复方法，所述摔倒自恢复系统具体包括：

控制器，所述控制器包括：

第一存储单元，用于在所述高智能机器人的运动过程中，按照所述高智能机器人的运动时刻对处理得到的所述实时运动轨迹进行保存；

第二存储单元，用于保存预先生成的若干摔倒恢复数据集合，每个所述摔倒恢复数据集合中包含预先生成的一标准摔倒姿势，以及根据所述标准摔倒姿势和预设的标准站立姿势生成的恢复动作序列；

姿势匹配单元，连接所述第二存储单元，用于根据所述告警提示通过所述惯性测量单元获取所述高智能机器人的当前摔倒姿势，分别计算所述当前摔倒姿势与各所述摔倒恢复数据集合中的所述标准摔倒姿势之间的匹配度并依次加入一匹配度序列，进而提取所述匹配度序列中所述匹配度最高的所述摔倒恢复数据集合并输出；

第一恢复单元，连接所述姿势匹配单元，用于根据所述当前摔倒姿势和提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势，控制各所述驱动关节动作，以控制所述高智能机器人由所述当前摔倒姿势恢复成所述标准摔倒姿势；

第二恢复单元，分别连接所述姿势匹配单元和所述第一恢复单元，用于根据提取得到的所述摔倒恢复数据集合中的所述标准摔倒姿势对应的所述恢复动作序列，控制各所述驱动关节根据所述恢复动作序列进行相应动作，以控制所述高智能机器人由所述标准摔倒姿势恢复成所述标准站立姿势；

第三恢复单元，分别连接所述第一存储单元和所述第二恢复单元，用于获取所述高智能机器人在摔倒前一所述运动时刻对应的所述实时运动轨迹，以表征所述高智能机器人的摔倒前姿势，并控制所述高智能机器人由所述标准站立姿势恢复成所述摔倒前姿势。

Images