CN111113378B - 基于三轴角速度防摔保护的机器人控制方法 - Google Patents

Abstract

一种基于三轴角速度防摔保护的机器人控制方法，涉及一种机器人控制方法，该方法是在基于三轴角速度防摔保护的机器人的每个机器人运动关节都安装有三轴角速度传感器，该三轴角速度传感器采集参数包含X、Y、Z个方向的角速度值；在机器人运行时，三轴角速度传感器实时采集运行时各个关节的三轴角速度值，通过数据处理后得到实时三轴角速度值，求取实时三轴角速度值与标准三轴角速度值的比值，并根据比值大小判断并执行相应的指令。本发明的准确性比较高，可避免阻碍人形机器人做高难度动作的不足之处，可对机器人运行时突然掉电摔倒进行保护，还可对电机和齿轮以及机身起到保护，可减少维修人员工作量，其结构简单，成本低，易于推广使用。

Description

基于三轴角速度防摔保护的机器人控制方法

技术领域

本发明涉及一种机器人及其控制方法，特别是一种基于三轴角速度防摔保护的机器人控制方法。

背景技术

目前，随着科技技术的快速发展，机器人产业得到迅猛发展，市面上随处可见运动机器人、服务机器人、教育机器人、唱歌跳舞机器人、对话机器人等。授权公告号为CN107962595B的发明专利公开了一种机器人倒地保护系统及方法，该机器人采用平衡感应模块和碰撞检测模块采集信号，当平衡信号和/或碰撞的强度信号高于阈值就报警；当平衡信号高于阈值才对碰撞的强度信号进行判断是否高于碰撞强度阈值，只有平衡信号和碰撞的强度信号都高于阈值才进行断电；发送求助信息包括地图位置信息、图像信息以及声音信息的至少一种。该方法存在的缺陷主要有：1.当人形机器人跳舞时，做翻滚动作或者倒立动作，平衡感应模块采集的平衡信号就会超过阀值，立刻发送报警信号，严重阻碍人形机器人做高难度动作；2.只有平衡信号和碰撞的强度信号都高于阈值才进行断电，只能对碰撞起到保护作用，无法对机器人运行时突然掉电摔倒起到保护，因为突然供电不足自己摔倒，所以平衡信号高于阀值，碰撞信号低于超过阀值，只有机器人摔倒和地面发生碰撞时，碰撞信号才高于阀值；3.发送的求助信号只精准到位置信息，没有办法精准到每个电机，增加检修人员工作量；4.当平衡信号和碰撞的强度信号都高于阈值直接断电，只是起到防止机器人燃烧，无法对电机、齿轮和机身起到保护，因为直接断电机器人立刻摔倒，电机和齿轮很容易损坏，甚至机身结构产生形变。

发明内容

本发明要解决的技术问题是：提供一种基于三轴角速度防摔保护的机器人控制方法，以解决现有机器人存在的严重阻碍人形机器人做高难度动作、无法对机器人运行时突然掉电摔倒起到保护、无法对电机和齿轮以及机身起到保护的不足之处。

解决上述技术问题的技术方案是：一种基于三轴角速度防摔保护的机器人控制方法，该方法包括以下步骤：

a).安装三轴角速度传感器

在基于三轴角速度防摔保护的机器人的每个机器人运动关节都安装三轴角速度传感器，该三轴角速度传感器采集参数包含X、Y、Z个方向的角速度值；

b). 获取标准三轴角速度值

机器人在完成调试时，通过三轴角速度传感器采集各个机器人运动关节的初始三轴角速度值β1Xi、β1Yi、β1Zi（i为1、2…m），通过数据处理后得到标准三轴角速度值N1并存储在存储单元中，其中，

N1=（β1Xi*λ1+β1Yi*λ2+β1Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

c). 获取实时三轴角速度值

在机器人运行时，三轴角速度传感器实时采集运行时各个关节的三轴角速度值β2Xi、β2Yi、β2Zi（i为1、2…m），通过数据处理后得到实时三轴角速度值N2，其中，

N2=（β2Xi*λ1+β2Yi*λ2+β2Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

d).求取实时三轴角速度值N2与标准三轴角速度值N1的比值

求取实时三轴角速度值N2与标准三轴角速度值N1的比值：

P=N2/N1；

e).实时判断并执行相应的指令

当a＜P＜b(其中0.6＜a＜1.0, 1.0＜b＜1.4)时,机器人正常工作，保持监测状态，继续重复步骤执行c)；当P≤a或者P≥b时，判断为机器人即将摔倒，继续步骤f)；

f). 启动保护系统和断开运动执行单元的电源

由中央控制器输出控制信号，通过驱动单元B启动保护系统, 同时通过电池管理单元断开运动执行单元的电源，即所有电机停止工作；

g). 机器人复位

当故障机器人排除故障后，进行复位操作，所有运行执行单元恢复初始状态。

本发明的进一步技术方案是：步骤f)中的启动保护系统的具体过程为：通过驱动单元B控制真空泵的正向电磁阀关闭、反向电磁阀打开和电机反转，气囊袋膨胀后真空泵的正向电磁阀和反向电磁阀都关闭，并有保护电机停止转动，进而达到保护的效果。

本发明的进一步技术方案是：在步骤f)中，还同时将包括图片信息、位置信息、故障机器人编号、故障伺服电机编号在内的故障信息发给后台管控单元、移动设备。

本发明的进一步技术方案是：在步骤f)中，还通过报警信息发送单元发送报警信息至分别安装在机器人机身系统、后台管控单元、移动设备上的声光报警器进行声光报警。

本发明的进一步技术方案是：在步骤g)中，保护系统进行复位操作的具体过程是：通过驱动单元B控制真空泵的正向电磁阀打开、反向电磁阀关闭和保护电机正转，当气囊袋收缩回机器人机身系统的U型槽内时，关闭正向电磁阀和反向电磁阀，且保护电机停止转动。

由于采用上述结构，本发明之基于三轴角速度防摔保护的机器人控制方法与现有技术相比，具有以下有益效果：

1.可避免阻碍人形机器人做高难度动作

由于本发明是在基于三轴角速度防摔保护的机器人的每个机器人运动关节都安装有三轴角速度传感器，该三轴角速度传感器采集参数包含X、Y、Z个方向的角速度值；在机器人运行时，三轴角速度传感器实时采集运行时各个关节的三轴角速度值，通过数据处理后得到实时三轴角速度值，求取实时三轴角速度值与标准三轴角速度值的比值，并根据比值大小判断并执行相应的指令。因此，本发明是根据三轴角速度传感器采集到的X、Y、Z个方向的角速度值进行判断机器人是否准备摔倒，其准确性比较高，避免了现有技术中采用平衡感应模块采集的平衡信号作为判断依据时，当机器人做翻滚动作或者倒立动作时，就会超过阀值而立刻发送报警信号导致的严重阻碍人形机器人做高难度动作的不足之处。

2. 可对机器人运行时突然掉电摔倒起到保护

本发明是根据三轴角速度传感器采集到的X、Y、Z个方向的角速度值进行判断机器人是否准备摔倒，当检测判断到机器人准备摔倒时，由中央控制器输出控制信号，通过驱动单元B启动保护系统。因此，本发明不仅对碰撞起到保护作用，还可对突然供电不足而对机器人进行保护，避免了现有技术中因突然掉电摔倒而损坏机器人的不足之处。

3. 可对电机和齿轮以及机身起到保护

本发明是根据三轴角速度传感器采集到的X、Y、Z个方向的角速度值进行判断机器人是否准备摔倒，当检测判断到机器人准备摔倒时，由中央控制器输出控制信号，通过驱动单元B启动保护系统，同时通过电池管理单元断开运动执行单元，即所有电机停止工作。因此，本发明一旦检测到机器人准备摔倒时，就启动保护系统进行保护机器人，可对电机和齿轮以及机身起到很好的保护作用。避免了现有技术中的当平衡信号和碰撞的强度信号都高于阈值直接断电，无法对电机、齿轮和机身起到保护，电机和齿轮很容易损坏，甚至机身结构产生形变的不足之处。

4.可减少维修人员工作量：

本发明在检测判断到机器人准备摔倒时，还同时将包括故障信息包括图片信息、位置信息、故障机器人编号、故障伺服电机编号在内的故障信息发给后台管控单元、移动设备；通过报警信息发送单元发送报警信息至分别安装在机器人机身系统、后台管控单元、移动设备上的声光报警器进行声光报警。因此，本发明不仅可以精准定位到损坏的机器人，而且可以精准定位到机器人损坏的电机，从而达到减少维修人员工作量，提高工作效率，减少运营成本的效果。

5.结构简单，成本低：

本发明的结构采用比较简单，生产成本较低，易于推广使用。

下面，结合附图和实施例对本发明之基于三轴角速度防摔保护的机器人控制方法的技术特征作进一步的说明。

附图说明

图1：本发明之基于三轴角速度防摔保护的机器人控制方法的流程图，

图2：实施例一所述基于三轴角速度防摔保护的机器人的结构示意图，

图3：实施例一所述控制系统的结构框图，

图4：实施例一所述保护系统的结构示意图之一（气囊袋处于打开状态），

图5：实施例一所述保护系统的结构示意图之二（气囊袋处于收缩状态），

图6：实施例一所述保护系统对机器人本体进行保护的示意图；

图中的各标号说明如下：

1-机器人腿部系统，

2-机器人机身系统，201-U型槽，

3-机器人手部系统，

4-机器人头部系统，

5-控制系统，

501-存储单元，502-移动设备，503-后台管控单元，504-三轴角速度传感器，

505-中央处理器，506-电源管理单元，507-驱动单元A，508-驱动单元B，

509-报警信息发送单元，510-声光报警器，

6-机器人电源系统，

7-保护系统，701-气囊袋，702-拉绳，703-保护电机，704-真空泵，

8-关节电机。

具体实施方式

实施例一：

一种基于三轴角速度防摔保护的机器人控制方法，该方法包括以下步骤：

a).安装三轴角速度传感器

在基于三轴角速度防摔保护的机器人的每个机器人运动关节都安装三轴角速度传感器，该三轴角速度传感器采集参数包含X、Y、Z个方向的角速度值；

b). 获取标准三轴角速度值

机器人在完成调试时，通过三轴角速度传感器采集各个机器人运动关节的初始三轴角速度值β1Xi、β1Yi、β1Zi（i为1、2…m），通过数据处理后得到标准三轴角速度值N1并存储在存储单元中，其中，

N1=（β1Xi*λ1+β1Yi*λ2+β1Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

c). 获取实时三轴角速度值

在机器人运行时，三轴角速度传感器实时采集运行时各个关节的三轴角速度值β2Xi、β2Yi、β2Zi（i为1、2…m），通过数据处理后得到实时三轴角速度值N2，其中，

N2=（β2Xi*λ1+β2Yi*λ2+β2Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

d).求取实时三轴角速度值N2与标准三轴角速度值N1的比值

求取实时三轴角速度值N2与标准三轴角速度值N1的比值：

P=N2/N1；

e).实时判断并执行相应的指令

当a＜P＜b(其中0.6＜a＜1.0, 1.0＜b＜1.4)时,机器人正常工作，保持监测状态，继续重复步骤执行c)；当P≤a或者P≥b时，判断为机器人即将摔倒，继续步骤f)；

f). 启动保护系统和断开运动执行单元的电源

由中央控制器输出控制信号，通过驱动单元B启动保护系统, 同时通过电池管理单元断开运动执行单元的电源，即所有电机停止工作；还同时将包括图片信息、位置信息、故障机器人编号、故障伺服电机编号在内的故障信息发给后台管控单元、移动设备；还通过报警信息发送单元发送报警信息至分别安装在机器人机身系统、后台管控单元、移动设备上的声光报警器进行声光报警。

g). 机器人复位

当故障机器人排除故障后，进行复位操作，所有运行执行单元恢复初始状态。

在步骤e)中，启动保护系统的具体过程为：通过驱动单元B控制真空泵的正向电磁阀关闭、反向电磁阀打开和电机反转，气囊袋膨胀后真空泵的正向电磁阀和反向电磁阀都关闭，并有保护电机停止转动，进而达到保护的效果。

在步骤f)中，保护系统进行复位操作的具体过程是：通过驱动单元B控制真空泵的正向电磁阀打开、反向电磁阀关闭和保护电机正转，当气囊袋收缩回机器人机身系统的U型槽内时，关闭正向电磁阀和反向电磁阀，且保护电机停止转动。

所述的基于三轴角速度防摔保护的机器人包括机器人腿部系统1、机器人机身系统2、机器人手部系统3、机器人头部系统4、控制系统5、机器人电源系统6、保护系统7；该机器人是具有与人体运动关节一致的机器人运动关节，每个机器人运动关节均有一个关节电机8；所述的控制系统5包括存储单元501、移动设备502、后台管控单元503、三轴角速度传感器504、中央处理器505、电源管理单元506、驱动单元A507、驱动单元B508、报警信息发送单元509；其中：

所述的存储单元501的信号输入输出端与中央处理器505的信号输入输出端连接；所述的三轴角速度传感器504的信号输出端与中央处理器505的信号输入端连接，中央处理器505的信号输出端分别与电源管理单元506、驱动单元A507、驱动单元B508的信号输入端，电源管理单元506、驱动单元A507的信号输出端分别与运动执行单元的信号输入端连接，驱动单元B508的信号输出端与保护系统7的信号输入端连接。

所述的报警信息发送单元509的信号输入端与中央处理器505的信号输出端连接，报警信息发送单元509的信号输出端与分别安装在机器人机身系统2、后台管控单元503、移动设备502上的声光报警器510进行通信连接；后台管控单元503、移动设备502的信号输入输出端与中央处理器505的信号输入输出端连接。

所述的机器人机身系统2上还设置有U型槽201，所述的保护系统7包括气囊袋701、拉绳702、保护电机703、真空泵704，气囊袋701的底端安装在机器人机身系统的U型槽21内，拉绳702的底端固定在保护电机703的输出端，拉绳702的顶端内置固定在气囊袋701的顶端，保护电机703、真空泵704分别安装在机器人机身系统2的U型槽201底部，该保护电机703、真空泵704的信号输入端分别与驱动单元B5027的信号输出端连接，真空泵704的输出端与气囊袋701连接。

上述的运动执行单元包括所述的机器人腿部系统1、机器人机身系统2、机器人手部系统3、机器人头部系统4，且该机器人腿部系统1、机器人机身系统2、机器人手部系统3、机器人头部系统4均属于现有技术。

上述的存储单元501、中央处理器505、驱动单元A507、驱动单元B508均采用常用的电子元器件，电源管理单元506为常用的电源开关，报警信息发送单元509为常用的无线发射器。

Claims (5)

1.一种基于三轴角速度防摔保护的机器人控制方法，其特征在于：该方法包括以下步骤：

a).安装三轴角速度传感器

在基于三轴角速度防摔保护的机器人的每个机器人运动关节都安装三轴角速度传感器，该三轴角速度传感器采集参数包含X、Y、Z个方向的角速度值；

b). 获取标准三轴角速度值

机器人在完成调试时，通过三轴角速度传感器采集各个机器人运动关节的初始三轴角速度值β1Xi、β1Yi、β1Zi，i为1、2…m，通过数据处理后得到标准三轴角速度值N1并存储在存储单元中，其中，

N1=（β1Xi*λ1+β1Yi*λ2+β1Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

c). 获取实时三轴角速度值

在机器人运行时，三轴角速度传感器实时采集运行时各个关节的三轴角速度值β2Xi、β2Yi、β2Zi，i为1、2…m，通过数据处理后得到实时三轴角速度值N2，其中，

N2=（β2Xi*λ1+β2Yi*λ2+β2Zi*λ3）/(λ1+λ2+λ3),

其中λ1、λ2、λ3为权重因子，0＜λ1＜3 , 0＜λ2＜3,0＜λ3＜1；

d).求取实时三轴角速度值N2与标准三轴角速度值N1的比值

求取实时三轴角速度值N2与标准三轴角速度值N1的比值：

P=N2/N1；

e).实时判断并执行相应的指令

当a＜P＜b时, 其中0.6＜a＜1.0, 1.0＜b＜1.4，机器人正常工作，保持监测状态，继续重复步骤执行c)；当P≤a或者P≥b时，判断为机器人即将摔倒，继续步骤f)；

f). 启动保护系统和断开运行执行单元的电源

由中央控制器输出控制信号，通过驱动单元B启动保护系统, 同时通过电池管理单元断开运行执行单元的电源，即所有电机停止工作；

g). 机器人复位

当故障机器人排除故障后，进行复位操作，所有运行执行单元恢复初始状态。

2.根据权利要求1所述的基于三轴角速度防摔保护的机器人控制方法，其特征在于：步骤f)中的启动保护系统的具体过程为：通过驱动单元B控制真空泵的正向电磁阀关闭、反向电磁阀打开和电机反转，气囊袋膨胀后真空泵的正向电磁阀和反向电磁阀都关闭，并有保护电机停止转动，进而达到保护的效果。

3.根据权利要求1所述的基于三轴角速度防摔保护的机器人控制方法，其特征在于：在步骤f)中，还同时将包括图片信息、位置信息、故障机器人编号、故障伺服电机编号在内的故障信息发给后台管控单元和移动设备。

4.根据权利要求1所述的基于三轴角速度防摔保护的机器人控制方法，其特征在于：在步骤f)中，还通过报警信息发送单元发送报警信息至分别安装在机器人机身系统、后台管控单元、移动设备上的声光报警器进行声光报警。

5.根据权利要求1所述的基于三轴角速度防摔保护的机器人控制方法，其特征在于：在步骤g)中，保护系统进行复位操作的具体过程是：通过驱动单元B控制真空泵的正向电磁阀打开、反向电磁阀关闭和保护电机正转，当气囊袋收缩回机器人机身系统的U型槽内时，关闭正向电磁阀和反向电磁阀，且保护电机停止转动。

Images