一种爬行机器人的摔倒检测方法及系统

技术领域

本发明涉及机器人检测技术领域，尤其涉及一种爬行机器人的摔倒检测方法及系统。

背景技术

现有的爬行机器人能够在非特定环境下行走，以完成给定的任务动作，由于行走环境是未知的，有些情况下难免会出现摔倒问题，进而无法正确执行后续任务动作，同时摔倒容易引起机器人损伤，因此亟需一种能够及时检测爬行机器人的摔倒状态，从而爬行机器人的操作员可以根据检测结果实时调整爬行机器人的任务动作。

发明内容

针对现有技术中存在的问题，本发明提供一种爬行机器人的摔倒检测方法，所述爬行机器人的每个机械臂具有至少一组驱动装置，所述驱动装置连接所述爬行机器人的控制器，每组所述驱动装置包括一驱动电机，以及连接所述驱动电机的一驱动关节；

所述摔倒检测方法具体包括以下步骤：

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

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

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

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

步骤S3，所述控制器控制所述驱动装置停止动作并输出表征所述爬行机器人处于摔倒状态的告警提示，以供所述爬行机器人的操纵者进行查看处理。

优选的，所述爬行机器人具有一姿态感应装置，连接所述控制器；

则所述步骤S2中，若不存在所述位置差值大于所述第一差值阈值，还包括：

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

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

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

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

步骤A2，所述控制器通过所述姿态感应装置获取所述爬行机器人的实时俯仰角，并将所述实时俯仰角与预设的俯仰角阈值进行比较：

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

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

优选的，每组所述驱动装置还包括一离合器，所述驱动电机通过所述离合器连接所述驱动关节；

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

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

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

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

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

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

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

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

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

优选的，所述步骤B1中，根据以下公式计算得到所述实时扭矩：

其中，

T用于表示所述实时扭矩；

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

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

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

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

优选的，所述编码器包括增量编码器和/或绝对编码器。

优选的，所述姿态感应装置为设置于所述机器人的躯干部位的惯性测量单元。

一种爬行机器人的摔倒检测系统，应用以上任意一项所述的爬行机器人的摔倒检测方法，所述摔倒检测系统具体包括：

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

数据获取模块，用于获取每组所述驱动装置中的所述驱动关节的实时关节位置数据并输出；

第一处理模块，连接所述数据获取模块，用于分别计算各所述实时关节位置数据与对应连接所述驱动电机的预设电机位置数据之间的位置差值，并在存在所述位置差值大于预设的第一差值阈值时生成相应的处理结果；

告警提示模块，连接所述第一处理模块，用于根据所述处理结果控制所述驱动装置停止动作并输出表征所述爬行机器人处于摔倒状态的告警提示，以供所述爬行机器人的操纵者进行查看处理。

优选的，所述爬行机器人具有一姿态感应装置，连接所述控制器，则所述控制器还包括第二处理模块，分别连接所述第一处理模块和所述告警提示模块，所述第二处理模块具体包括：

数据比较单元，用于在不存在所述位置差值大于所述第一差值阈值时，将所述位置差值与预设的第二差值阈值进行比较，并在存在所述位置差值大于所述第二差值阈值时生成第一比较结果并输出；

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

第一处理单元，连接所述数据比较单元，用于根据所述第一比较结果通过所述姿态感应装置获取所述爬行机器人的实时俯仰角，并将所述实时俯仰角与预设的俯仰角阈值进行比较，并在所述实时俯仰角小于所述俯仰角阈值时生成所述处理结果并输出至所述告警提示模块。

优选的，每组所述驱动装置还包括一离合器，所述驱动电机通过所述离合器连接所述驱动关节，则所述第二处理模块还包括第二处理单元，连接所述第一处理单元，所述第二处理单元具体包括：

数据处理子单元，用于在所述实时俯仰角不小于所述俯仰角阈值时，获取所述驱动电机的输出电流，并根据所述输出电流计算得到所述离合器的实时扭矩；

第一比较子单元，连接所述数据处理子单元，用于将所述实时扭矩与预设的第一扭矩阈值进行比较，并在所述实时扭矩大于所述第一扭矩阈值时输出第二比较结果，以及在所述实时扭矩不大于所述第一扭矩阈值时生成所述处理结果并输出至所述告警提示模块；

第二比较子单元，连接所述第一比较子单元，用于根据所述第二比较结果将所述实时扭矩与预设的第二扭矩阈值进行比较，并在所述实时扭矩不小于所述第二扭矩阈值时生成所述处理结果并输出至所述告警提示模块；

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

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

1)能够及时检测爬行机器人的摔倒状态，有效提升爬行机器人的摔倒检测准确性的同时降低爬行机器人的摔倒误报率；

2)在检测到爬行机器人处于摔倒状态时，及时控制爬行机器人的驱动装置停止动作，以避免由于爬行机器人摔倒引起的驱动装置空转或打滑。

附图说明

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

图2为本发明的较佳的实施例中，不存在位置差值大于第一差值阈值时的处理流程图；

图3为本发明的较佳的实施例中，俯仰角不小于俯仰角阈值时的处理流程图；

图4为本发明的较佳的实施例中，一种爬行机器人的摔倒检测系统的结构示意图。

具体实施方式

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

本发明的较佳的实施例中，基于现有技术中存在的上述问题，现提供一种爬行机器人的摔倒检测方法，如图4所示，爬行机器人的每个机械臂具有至少一组驱动装置2，驱动装置2连接爬行机器人的控制器1，每组驱动装置2包括一驱动电机22，以及连接驱动电机22的一驱动关节21；

如图1所示，摔倒检测方法具体包括以下步骤：

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

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

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

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

步骤S3，控制器控制驱动装置停止动作并输出表征爬行机器人处于摔倒状态的告警提示，以供爬行机器人的操纵者进行查看处理。

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

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

本发明的较佳的实施例中，爬行机器人具有一姿态感应装置3，连接控制器1；

则步骤S2中，若不存在位置差值大于第一差值阈值，如图2所示，还包括：

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

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

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

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

步骤A2，控制器通过姿态感应装置获取爬行机器人的实时俯仰角，并将实时俯仰角与预设的俯仰角阈值进行比较：

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

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

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

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

本发明的较佳的实施例中，每组驱动装置2还包括一离合器23，驱动电机22通过离合器23连接驱动关节21；

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

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

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

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

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

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

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

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

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

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

本发明的较佳的实施例中，步骤B1中，根据以下公式计算得到实时扭矩：

其中，

T用于表示实时扭矩；

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

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

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

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

本发明的较佳的实施例中，编码器211包括增量编码器和/或绝对编码器。

本发明的较佳的实施例中，姿态感应装置3为设置于机器人的躯干部位的惯性测量单元。

一种爬行机器人的摔倒检测系统，应用以上任意一项的爬行机器人的摔倒检测方法，如图4所示，摔倒检测系统具体包括：

控制器1，控制器1具体包括：

数据获取模块11，用于获取每组驱动装置2中的驱动关节21的实时关节位置数据并输出；

第一处理模块12，连接数据获取模块12，用于分别计算各实时关节位置数据与对应连接驱动电机22的预设电机位置数据之间的位置差值，并在存在位置差值大于预设的第一差值阈值时生成相应的处理结果；

告警提示模块13，连接第一处理模块12，用于根据处理结果控制驱动装置2停止动作并输出表征爬行机器人处于摔倒状态的告警提示，以供爬行机器人的操纵者进行查看处理。

本发明的较佳的实施例中，爬行机器人具有一姿态感应装置3，连接控制器1，则控制器1还包括第二处理模块14，分别连接第一处理模块12和告警提示模块13，第二处理模块14具体包括：

数据比较单元141，用于在不存在位置差值大于第一差值阈值时，将位置差值与预设的第二差值阈值进行比较，并在存在位置差值大于第二差值阈值时生成第一比较结果并输出；

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

第一处理单元142，连接数据比较单元141，用于根据第一比较结果通过姿态感应装置3获取爬行机器人的实时俯仰角，并将实时俯仰角与预设的俯仰角阈值进行比较，并在实时俯仰角小于俯仰角阈值时生成处理结果并输出至告警提示模块。

本发明的较佳的实施例中，每组驱动装置2还包括一离合器23，驱动电机22通过离合器23连接驱动关节21，则第二处理模块14还包括第二处理单元143，连接第一处理单元142，第二处理单元143具体包括：

数据处理子单元1431，用于在实时俯仰角不小于俯仰角阈值时，获取驱动电机22的输出电流，并根据输出电流计算得到离合器23的实时扭矩；

第一比较子单元1432，连接数据处理子单元1431，用于将实时扭矩与预设的第一扭矩阈值进行比较，并在实时扭矩大于第一扭矩阈值时输出第二比较结果，以及在实时扭矩不大于第一扭矩阈值时生成处理结果并输出至告警提示模块13；

第二比较子单元1433，连接第一比较子单元1432，用于根据第二比较结果将实时扭矩与预设的第二扭矩阈值进行比较，并在实时扭矩不小于第二扭矩阈值时生成处理结果并输出至告警提示模块13；

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

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