CN112247986A - 一种用于床面自主移动机器人的弓字形路径规划控制方法 - Google Patents

Abstract

本发明公开了一种用于床面自主移动机器人的弓字形路径规划控制方法，属于路径规划方法领域，所述控制方法包括如下步骤：机器人放到床面上后，先进行机器热身运动，热身运动完成后进行状态校准和陀螺仪校准，校准完成后，控制直线运动，判断是否到悬崖或者障碍物，根据悬崖或者障碍物的位置设定距离进行后退避障处理，如果直线运动的时间超过最大直线运动时间或避障完成后，机器人控制转向运动，如果转向过程中遇到障碍时，进行避障处理后在转向运动。本发明运算速度快，对芯片运算性能依赖性低，覆盖效率高，可保障整个床面的全服高，悬崖规避性能好，跌落概率低，杀菌效果更好使用直线运动和转向切换完成弓字形路线，设备在床面最大运行速度为0.1m/s。

Description

一种用于床面自主移动机器人的弓字形路径规划控制方法

技术领域

本发明涉及路径规划方法领域，尤其涉及一种用于床面自主移动机器人的弓字形路径规划控制方法。

背景技术

当前应用于床面杀菌除螨机器人的规划及运动方法有以下缺陷：

行业内普遍使用随机路径，即遇到碰撞或跌落后随机角度折返，覆盖床面的效率低下、体验较差，并且无法确保床面每一处都覆盖到了。行业内的运动控制方法跌落床面的概率较大，容易损坏机器人，同时杀菌除螨效率低下，效果不好，给用户带来不好的体验。同时移动速度过快导致杀菌效果较差等。因此，需要设计一种高效率的弓字形运动，对床面进行完全覆盖，同时避开床面障碍物以及防止机器人跌落床面的控制方法。

发明内容

本发明的目的在于提供一种用于床面自主移动机器人的弓字形路径规划控制方法，解决背景技术中存在的技术问题。

该方法旨在解决智能床面杀菌、除螨机器人在床面进行自主运动的路径规划及控制问题。基于陀螺仪、地面检测红外传感器的反馈数据进行算法处理，对行走路径及动力轮进行规划控制，使机器能在床上完成高效率的弓字形运动，对床面进行完全覆盖，同时避开床面障碍物以及防止机器人跌落床面。

一种用于床面自主移动机器人的弓字形路径规划控制方法，所述控制方法包括如下步骤：

步骤1：机器人放到床面上后，先进行机器热身运动；

步骤2：热身运动完成后进行状态校准和陀螺仪校准；

步骤3：校准完成后，控制直线运动，判断是否到悬崖或者障碍物，根据悬崖或者障碍物的位置设定距离进行后退避障处理；

步骤4：如果步骤3中的直线运动的时间超过最大直线运动时间或避障完成后，机器人控制转向运动，如果转向过程中遇到障碍时，进行避障处理后在转向运动，转向运动完后进入步骤2。

进一步地，所述步骤1中的具体过程为：陀螺仪校准期间需要机器静止 1-3s，通过原地左转30°再右转60°再左转30°结束热身运动进入静止，时间为 1-3s。

进一步地，所述步骤2中的状态校准和陀螺仪校准的具体过程为：

状态校准，在进入直线运动前对陀螺仪进行校准，每次校准后20s内不在进行校准，即校准动作的冷却，则跳过校准进入直线运行，由于机器人所处环境不一定是稳定静止的，如果进入校准状态5s内没有完成校准计算即校准超时，则放弃校准，进入直线运行模式；

陀螺仪校准，由于陀螺仪各轴在静止时，存在零点漂移，在运行过程中存在温度漂移，其中俯仰角和翻滚角可以通过垂直方向上的加速度进行互补滤波消除漂移，但偏航角不能，需要在开始运动之前，设备静止时进行陀螺仪校准，并在运行过程中适时的停下机器进行校准，进入校准时，关闭轮子，对6轴数据分别进行采样，并分别记录前20次的采样记录，如果单个轴上20 次采样结果的方差小于5，则认为该轴处于静止状态，如果6个轴都处于静止状态，则分别对各个轴上20次的采样结果取平均值作为其轴的漂移校准值，更新旧的校准值之后，开启轮子正常运行，进入校准时间为，进入运动工作模式后，进行正式运动之前或者弓字形运动过程中，完成转向在走直线之前。

进一步地，所述步骤3控制直线运动的具体过程为：

直线行走的角度仅有0°和180°两种，在进入直线行走时，根据上一次的角度设定本次直线行走的目标角度，如果上一次时0°，则本次为180°，反之，上一次为180°本次就为0°，行走角度通过PD控制，根据当前偏航角与目标角度的夹角以及当前角速度计算左右轮轮数差，计算公式如下：

dv＝p×(2×MaxPWM/MaxAngle)×e-d×AngleSpd

其中：p为比例权重，根据实验设定为0.5，d为微分权重，根据实验设定为0.01，e为当前角度与目标角度的夹角，当前角度在目标角度左侧则e为正值，否则为负值，MaxPWM为可设置的最大占空比数值，MaxAngle为最大控制角度，如果|e|大于MaxAngle，则直接进行原地转向，建议设定为 15°-30°，AngleSpd为陀螺仪读取的当前偏航角速度，计算出速度差后，对速度差进行限幅后设置期望占空比，再通过速度闭环控制函数计算最终设置的占空比进行设置。

进一步地，所述步骤3中，避障运动的具体过程为：

转向方式为左转和右转，左转的控制为左轮占空比-100何右轮占空比500，右转的控制为左轮占空比500和右轮占空比-100，如果上一次直线运行时间小于上上次直线运行时间且上次直线运行时间小于5s则不切换转向方式，使用与上一次相同的转向方式，否则，使用与上一次不同的转向方式，如果到床边后运行线路与床边平行，无法触发上述逻辑进行掉头，，如果在转向过程中遇到跌落，则下次转向不切换转向方向，使机器掉头；

当机器角度与下一条线路目标角度的夹角小于30°时，停止转向，切换为直线运动控制。

进一步地，所述步骤4中控制转向运动的具体过程为：

悬崖位置表达为以轮轴中间点为坐标原点，机器头部朝向方向为0°角度构建机器局部极坐标，以局部极坐标的角度θ表示悬崖的方位，

悬崖位置判断：如果倾斜，则通过陀螺仪翻滚角及俯仰角确定悬崖方向，悬崖相对于机器头部方向的夹角θ的正切等于俯仰角除以翻滚角，故可得公式：

如果没有倾斜，只有红外地检触发，则根据所有触发地检在机器局部极坐标中的角度的平均角度作为悬崖方位，

由于倾斜时，所计算出来的悬崖相对于机器的位置，是近似垂直于床边的，故返回床面时，锁定机器的运行角度到悬崖所在方向进行后退运动，障碍的判断仅在直线运动及转向控制中没有检测到跌落的情况下处理，判断依据为：直线运动2s内位移小于5mm；转向运动中2s内角度小于5°；检测到轮子电流超过设定保护值2s；机器前进方向加速度向前进方向反向突变，加速度为x 轴加速度，判断到障碍物后保持当前角度进行后退，

避障的结束：在后退过程中不断采样前进方向上的加速度，积分后得到各个时刻的速度，对速度进行积分计算后退的位移，当后退位移大于5cm且红外地检正常则完成悬崖规避。

进一步地，控制直线运动的速度为0.1m/s。

本发明采用了上述技术方案，本发明具有以下技术效果：

本发明运算速度快，对芯片运算性能依赖性低，覆盖效率高，可保障整个床面的全服高，悬崖规避性能好，跌落概率低，杀菌效果更好使用直线运动和转向切换完成弓字形路线，设备在床面最大运行速度为0.1m/s，直线运动控制采用方案中所述PD控制公式计算左右轮速差，是机器保持贴近目标直线运动，通过动力轮电流、加速度计、角速度计进行障碍物判断，在避开障碍物或悬崖时通过计算障碍物相对当前机器的角度，确定避开障碍物的后退路线，实现高成功率的悬崖规避。

附图说明

图1为本发明控制流程图。

图2为本发明路径规划示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，举出优选实施例，对本发明进一步详细说明。然而，需要说明的是，说明书中列出的许多细节仅仅是为了使读者对本发明的一个或多个方面有一个透彻的理解，即便没有这些特定的细节也可以实现本发明的这些方面。

一种用于床面自主移动机器人的弓字形路径规划控制方法，如图1所示，所述控制方法包括如下步骤：

步骤1：机器人放到床面上后，先进行机器热身运动。由于陀螺仪校准期间需要机器静止1～3s，为避免用户误解机器没有按预期进入运动模式，在进入弓字行运动时，先进行热身运动后在静止机器进行校准。通过原地左转 30°再右转60°再左转30°结束热身运动进入静止。

步骤2：热身运动完成后进行状态校准和陀螺仪校准。

校准状态

如上文所述，为提高陀螺仪导航精度，在进入直线运动前对陀螺仪进行校准。每次校准后20s内不在进行校准，即校准动作的冷却，则跳过校准步骤进入直线运行。由于机器人所处环境不一定是稳定静止的，如果进入校准状态5s内没有完成校准计算即校准超时，则放弃校准，进入直线运行模式。

陀螺仪校准方法

由于陀螺仪各轴在静止时，存在零点漂移，在运行过程中存在温度漂移，其中俯仰角和翻滚角可以通过垂直方向上的加速度进行互补滤波消除漂移，但偏航角不能。为提升陀螺仪精度，需要在开始运动之前，设备静止时进行陀螺仪校准，并在运行过程中适时的停下机器进行校准。

进入校准时，关闭轮子，对6轴数据分别进行采样，并分别记录前20 次的采样记录，如果单个轴上20次采样结果的方差小于5(后续根据实验结果调整)，则认为该轴处于静止状态，如果6个轴都处于静止状态，则分别对各个轴上20次的采样结果取平均值作为其轴的漂移校准值，更新旧的校准值之后，开启轮子正常运行。

进入校准时机如下：

进入运动工作模式后，进行正式运动之前

弓字形运动过程中，完成转向在走直线之前(如果后续测试校准花费时间较多，影响体验，可设置校准冷却时间，即在校准后一定时间内不再进行校准)。

步骤3：校准完成后，控制直线运动，判断是否到悬崖或者障碍物，根据悬崖或者障碍物的位置设定距离进行后退避障处理。

直线运行状态与走直线控制方法

直线行走的角度仅有0°和180°两种，在进入直线行走时，根据上一次的角度设定本次直线行走的目标角度，如果上一次时0°，则本次为180°，反之，上一次为180°本次就为0°。行走角度通过PD控制，根据当前偏航角与目标角度的夹角以及当前角速度计算左右轮轮数差。计算公式如下

dv＝p×(2×MaxPWM/MaxAngle)×e-d×AngleSpd

其中：

p为比例权重，根据实验设定为0.5。

d为微分权重，根据实验设定为0.01。

e为当前角度与目标角度的夹角，当前角度在目标角度左侧则e为正值，否则为负值。

MaxPWM为可设置的最大占空比数值。

MaxAngle为最大控制角度，如果|e|大于MaxAngle，则直接进行原地转向，建议设定为15°～30°。

AngleSpd为陀螺仪读取的当前偏航角速度。

计算出速度差后，对速度差进行限幅后设置期望占空比。再通过速度闭环控制函数计算最终设置的占空比进行设置。

避障

悬崖位置表达：以轮轴中间点为坐标原点，机器头部朝向方向为0°角度构建机器局部极坐标，以局部极坐标的角度θ表示悬崖的方位。

悬崖位置判断：

如果有倾斜，则通过陀螺仪翻滚角及俯仰角确定悬崖方向，悬崖相对于

并特殊处理roll为0的情况)

如果没有倾斜，只有红外地检触发，则根据所有触发地检在机器局部极坐标中的角度的平均角度作为悬崖方位。

由于倾斜时，所计算出来的悬崖相对于机器的位置，大部分情况是近似垂直于床边的，故返回床面时，锁定机器的运行角度到悬崖所在方向进行后退运动。

障碍的判断仅在直线运动及转向控制中没有检测到跌落的情况下处理，判断依据为：

直线运动2s内位移小于5mm

转向运动中2s内角度小于5°

检测到轮子电流超过设定保护值2s

机器前进方向加速度(x轴加速度)向前进方向反向突变，具体判断方式据实验结果调试。

判断到障碍物后保持当前角度进行后退。

避障的结束：在后退过程中不断采样前进方向上的加速度，积分后得到各个时刻的速度，对速度进行积分计算后退的位移。当后退位移大于5cm且红外地检正常则完成悬崖规避。

步骤4：如果步骤3中的直线运动的时间超过最大直线运动时间或避障完成后，机器人控制转向运动，如果转向过程中遇到障碍时，进行避障处理后在转向运动，转向运动完后进入步骤2。

转向控制

转向方式为左转(左轮占空比-100，右轮占空比500)和右转(左轮占空比500，右轮占空比-100)。

如果上一次直线运行时间小于上上次直线运行时间且上次直线运行时间小于5s则不切换转向方式，使用与上一次相同的转向方式，否则，使用与上一次不同的转向方式。

如果到床边后运行线路与床边平行，可能无法触发上述逻辑进行掉头，故，如果在转向过程中遇到跌落，则下次转向不切换转向方向，使机器掉头。

当机器角度与下一条线路目标角度的夹角小于30°时，停止转向，切换为直线运动控制。配合上文直线运动控制方案，可形成如图2所示路径。

该方法旨在解决智能床面杀菌、除螨机器人在床面进行自主运动的路径规划及控制问题。基于陀螺仪、地面检测红外传感器的反馈数据进行算法处理，对行走路径及动力轮进行规划控制，使机器能在床上完成高效率的弓字形运动，对床面进行完全覆盖，同时避开床面障碍物以及防止机器人跌落床面。在机器人底部，使用2个动力轮，1个万向轮以及3个地面检测红外传感器。使用直线运动和转向切换完成弓字形路线。

设备在床面最大运行速度为0.1m/s，直线运动控制采用方案中所述PD 控制公式计算左右轮速差，是机器保持贴近目标直线运动，通过动力轮电流、加速度计、角速度计进行障碍物判断。在避开障碍物或悬崖时通过计算障碍物相对当前机器的角度，确定避开障碍物的后退路线，实现高成功率的悬崖规避。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述控制方法包括如下步骤：

步骤1：机器人放到床面上后，先进行机器热身运动；

步骤2：热身运动完成后进行状态校准和陀螺仪校准；

步骤3：校准完成后，控制直线运动，判断是否到悬崖或者障碍物，根据悬崖或者障碍物的位置设定距离进行后退避障处理；

步骤4：如果步骤3中的直线运动的时间超过最大直线运动时间或避障完成后，机器人控制转向运动，如果转向过程中遇到障碍时，进行避障处理后在转向运动，转向运动完后进入步骤2。

2.根据权利要求1所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述步骤1中的具体过程为：陀螺仪校准期间需要机器静止1-3s，通过原地左转30°再右转60°再左转30°结束热身运动进入静止，时间为1-3s。

3.根据权利要求2所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述步骤2中的状态校准和陀螺仪校准的具体过程为：

状态校准，在进入直线运动前对陀螺仪进行校准，每次校准后20s内不在进行校准，即校准动作的冷却，则跳过校准进入直线运行，由于机器人所处环境不一定是稳定静止的，如果进入校准状态5s内没有完成校准计算即校准超时，则放弃校准，进入直线运行模式；

陀螺仪校准，由于陀螺仪各轴在静止时，存在零点漂移，在运行过程中存在温度漂移，其中俯仰角和翻滚角可以通过垂直方向上的加速度进行互补滤波消除漂移，但偏航角不能，需要在开始运动之前，设备静止时进行陀螺仪校准，并在运行过程中适时的停下机器进行校准，进入校准时，关闭轮子，对6轴数据分别进行采样，并分别记录前20次的采样记录，如果单个轴上20次采样结果的方差小于5，则认为该轴处于静止状态，如果6个轴都处于静止状态，则分别对各个轴上20次的采样结果取平均值作为其轴的漂移校准值，更新旧的校准值之后，开启轮子正常运行，进入校准时间为，进入运动工作模式后，进行正式运动之前或者弓字形运动过程中，完成转向在走直线之前。

4.根据权利要求3所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述步骤3控制直线运动的具体过程为：

直线行走的角度仅有0°和180°两种，在进入直线行走时，根据上一次的角度设定本次直线行走的目标角度，如果上一次时0°，则本次为180°，反之，上一次为180°本次就为0°，行走角度通过PD控制，根据当前偏航角与目标角度的夹角以及当前角速度计算左右轮轮数差，计算公式如下：

dv＝p×(2×MaxPWM/MaxAngle)×e-d×AngleSpd

其中：p为比例权重，根据实验设定为0.5，d为微分权重，根据实验设定为0.01，e为当前角度与目标角度的夹角，当前角度在目标角度左侧则e为正值，否则为负值，MaxPWM为可设置的最大占空比数值，MaxAngle为最大控制角度，如果|e|大于MaxAngle，则直接进行原地转向，建议设定为15°-30°，AngleSpd为陀螺仪读取的当前偏航角速度，计算出速度差后，对速度差进行限幅后设置期望占空比，再通过速度闭环控制函数计算最终设置的占空比进行设置。

5.根据权利要求4所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述步骤3中，避障运动的具体过程为：

转向方式为左转和右转，左转的控制为左轮占空比-100何右轮占空比500，右转的控制为左轮占空比500和右轮占空比-100，如果上一次直线运行时间小于上上次直线运行时间且上次直线运行时间小于5s则不切换转向方式，使用与上一次相同的转向方式，否则，使用与上一次不同的转向方式，如果到床边后运行线路与床边平行，无法触发上述逻辑进行掉头，，如果在转向过程中遇到跌落，则下次转向不切换转向方向，使机器掉头；

当机器角度与下一条线路目标角度的夹角小于30°时，停止转向，切换为直线运动控制。

6.根据权利要求5所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：所述步骤4中控制转向运动的具体过程为：

悬崖位置表达为以轮轴中间点为坐标原点，机器头部朝向方向为0°角度构建机器局部极坐标，以局部极坐标的角度θ表示悬崖的方位，

悬崖位置判断：如果倾斜，则通过陀螺仪翻滚角及俯仰角确定悬崖方向，悬崖相对于机器头部方向的夹角θ的正切等于俯仰角除以翻滚角，故可得公式：

如果没有倾斜，只有红外地检触发，则根据所有触发地检在机器局部极坐标中的角度的平均角度作为悬崖方位，

由于倾斜时，所计算出来的悬崖相对于机器的位置，是近似垂直于床边的，故返回床面时，锁定机器的运行角度到悬崖所在方向进行后退运动，障碍的判断仅在直线运动及转向控制中没有检测到跌落的情况下处理，判断依据为：

直线运动2s内位移小于5mm；转向运动中2s内角度小于5°；检测到轮子电流超过设定保护值2s；机器前进方向加速度向前进方向反向突变，加速度为x轴加速度，判断到障碍物后保持当前角度进行后退，

避障的结束：在后退过程中不断采样前进方向上的加速度，积分后得到各个时刻的速度，对速度进行积分计算后退的位移，当后退位移大于5cm且红外地检正常则完成悬崖规避。

7.根据权利要求5所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：控制直线运动的速度为0.1m/s。

Images