一种扫地机器人沿墙运动控制方法与流程

本发明涉及扫地机器人技术领域，具体为一种扫地机器人沿墙运动控制方法。

背景技术：

目前扫地机器人沿墙清扫路径规划方法存在以下缺陷：(1)在通过碰撞传感器(前挡)探测墙体到进入沿墙行走的过程中，需要进行多次碰撞，产生大量噪音，严重影响用户体验且使碰撞传感器寿命快速降低，导致整机可靠性降低；(2)沿墙行走时对墙体要求较高，目前大多数技术无法探测瓷砖、金属等镜面材质；(3)沿墙距离不稳定，其距离取决于沿墙红外信号阀值的设定，不同墙体由于反光性不同导致沿墙距离忽远忽近，造成清扫覆盖率低下；(4)沿墙行走时无法处理动态障碍，例如来自人、宠物或其他扫地机的干扰。

技术实现要素：

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种扫地机器人沿墙运动控制方法，解决了现有市面上出售的扫地机器人由于其使用的算法导致行走过程中需要多次碰撞导致对墙体要求较高、沿墙距离不稳定、无法处理动态障碍、寿命和可靠性较低以及用户体验感差的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：一种扫地机器人，包括扫地机本体和设置于扫地机本体前端的前挡，所述扫地机本体左右两侧中至少一侧设置有一用于探测墙面的沿墙传感器，所述前挡对应沿墙传感器开设有探测口，所述沿墙传感器包括红外发射源和红外接收器，所述红外发射源的发射基准线与红外接收器的接收基准线之间的夹角呈角度a设置，其中20°<a<40°,所述夹角的角平分线与扫地机器人前进方向垂直，所述红外发射源的发射基准线与红外接收器的接收基准线存在一交点，所述交点与前挡距离为l,其中l<1cm。

优选的，所述扫地机器人沿墙前进时，所述夹角的角平分线与被探测墙面垂直，且所述交点位于扫地机本体与被探测墙面之间。

优选的，所述l<5mm。

优选的，所述扫地机本体左右两侧均设置有沿墙传感器。

一种扫地机器人沿墙运动控制方法，包括如权利要求1-4任意一项所述的一种扫地机器人，所述扫地机本体内还设置有主控单元、pid稳定控制单元以及pid距离控制单元，所述前挡内设置有碰撞传感器，所述pid稳定控制单元、pid距离控制单元以及碰撞传感器均电性连接主控单元：

步骤1：开启扫地机器人，设定最小沿墙信号值min_ir和最大沿墙信号值max_ir，主控单元执行沿墙程序，驱动扫地机器人沿墙直线行走，沿墙传感器实时将沿墙信号值ir发送至主控单元；

步骤2：主控单元对实时接收到的沿墙信号值ir与最大沿墙信号值max_ir进行对比分析，选取最大值作为最大沿墙信号值max_ir，并取最大沿墙信号值max_ir的60％-80％作为目标信号值；

步骤3：主控单元计算出实时接收的沿墙信号值ir与上一次接收的沿墙信号值last_ir的差值，记为err_ir，发送至pid稳定控制单元，并同步计算出实时接收的沿墙信号值ir与目标信号值的差值，记为trg_err_ir,发送至pid距离控制单元；

步骤4：pid稳定控制单元接收到err_ir后计算出稳定轮速差dv1，并反馈至主控单元，pid距离控制单元接收到trg_err_ir后计算出距离轮速差dv2并同步反馈至主控单元；

步骤5：主控单元根据反馈结果计算出轮速差dv，并根据轮速差dv控制左、右驱动轮的轮速，调整扫地机器人的行进姿态以及扫地机器人与被探测墙面之间的距离。

步骤6：扫地机器人行走期间，若碰撞传感器发送碰撞信号至主控单元，主控单元则执行内转程序或避障程序，否则保持直行。

优选的，所述pid稳定控制单元接收到err_ir以及pid距离控制单元接收到trg_err_ir后，均先通过公式cof＝kp×δsn+ki×∑δs+kd×(δsn-δsn-1)计算出速度差值cof，式中，kp为比例控制权重，ki为积分控制权重，kd为微分控制权重，δsn为本次输入值，δsn-1为上一次的输入值；

再通过公式进行限幅运算，得出稳定轮速差dv1或距离轮速差dv2，式中，maxspeeddif为最大可设置的速度差，max_ir为采集到的最大信号，maxintegrate为设定的积分限制。

优选的，所述主控单元接收到pid稳定控制单元发送的dv1以及pid距离控制单元发送的dv2后，对max_ir与设定的动态权重临界点进行对比分析：

若max_ir>动态权重临界点，主控单元根据公式dv＝dv1×a+dv2×b,a＝0.15,b＝0.85计算出轮速差dv，否则主控单元根据公式dv＝dv1×a+dv2×b,a＝0.9,b＝0.1计算出轮速差dv；

主控单元设置远离被探测墙面的驱动轮轮速为可设置的最大轮速，另一驱动轮轮速为远离被探测墙面的驱动轮轮速和轮速差dv的差值。

优选的，所述主控单元执行内转程序时，将max_ir重置为0，并驱动扫地机器人朝沿墙方式相同方向转动，并检测是否有碰撞，若有碰撞则执行避障程序，若无碰撞，则继续转动直至发生碰撞。

优选的，所述主控单元执行避障程序，将max_ir重置为0，驱动扫地机器人后退一定距离，朝沿墙方式相反方向转动，转动期间，主控单元对实时接收的沿墙信号值ir进行判断分析，若max_ir>ir≥min_ir,主控单元执行沿墙程序，否则驱动扫地机器人继续转动。

优选的，所述主控单元执行沿墙程序，将max_ir重置为0，并通过pid稳定控制单元和pid距离控制单元控制扫地机器人沿墙运动，并检测是否有碰撞，若有碰撞则执行避障程序，否则主控单元对实时接收的沿墙信号值ir进行分析判断，若ir<min_ir或(last_ir-ir)/last_ir<0.5或ir/last_ir<0.2，则执行内转程序，否则继续前进。

(三)有益效果

本发明提供了一种基于pid控制算法的扫地机器人沿线行走控制系统及其控制方法。具备以下有益效果：

(1)成本低，对内存依赖极小，可在stm32f0x0芯片中与其他扫地机器人算法一同流畅运行；

(2)可通过1次碰撞完成墙体探测，快速调整机身与墙体方向平行；

(3)沿墙传感器布置结构优化合理；

(4)可在一次沿墙运动中根据不同墙体反光强度动态调整控制参考量，自动适应各种反光性能不同的墙体；

(5)使用pid距离控制单元控制沿墙距离，并在沿墙运动过程中进行绕圈判断，从而对动态障碍及干扰具有良好的适应性。

附图说明

图1为本发明的扫地机本体硬件结构示意图；

图2为本发明的沿墙传感器结构示意图；

图3为本发明的扫地机本体外观结构示意图；

图4为本发明的沿墙工作状态切换流程框图；

图5为本发明的双pid动态权重沿墙运动控制流程框图。

图中，1扫地机本体、2沿墙传感器、3前挡、4红外接收器、5红外发射源、6探测口。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-5，本发明实施例提供一种技术方案，一种扫地机器人，包括扫地机本体1和设置于扫地机本体1前端的前挡3，前挡3呈半圆弧形设置，半径略大于扫地机本体1的半径，罩设于扫地机本体1前端，所述扫地机本体1左右两侧中至少一侧设置有一用于探测墙面的沿墙传感器2，所述前挡3对应沿墙传感器2开设有探测口6，使得红外发射源5发射的红外线能够不被遮挡的照射在被探测的墙面上，并返回被红外接收器4接收，所述沿墙传感器2包括红外发射源5和红外接收器4，所述红外发射源5的发射基准线与红外接收器4的接收基准线之间的夹角呈角度a设置，其中20°<a<40°,所述夹角的角平分线与扫地机器人前进方向垂直，所述红外发射源5的发射基准线与红外接收器4的接收基准线存在一交点，所述交点与前挡3距离为l,其中l<1cm。

所述扫地机器人沿墙前进时，所述夹角的角平分线与被探测墙面垂直，且所述交点位于扫地机本体1与被探测墙面之间。

所述l<5mm。

所述扫地机本体1左右两侧均设置有沿墙传感器2，即该扫地机器人能够进行左沿墙行走(扫地机本体1左边靠墙进行直线行走)和右沿墙行走(扫地机本体1右边靠墙进行直线行走)。

本发明还提供一种扫地机器人沿墙运动控制方法，包括如权利要求1-4任意一项所述的一种扫地机器人，所述扫地机本体1内还设置有主控单元、pid稳定控制单元(稳定性优先的pid控制单元)以及pid距离控制单元(沿墙距离有限的pid控制单元)，所述前挡3内设置有用于检测碰撞的碰撞传感器，所述pid稳定控制单元、pid距离控制单元以及碰撞传感器均电性连接主控单元：

步骤1：开启扫地机器人，设定最小沿墙信号值min_ir和最大沿墙信号值max_ir，主控单元执行沿墙程序，驱动扫地机器人沿墙直线行走，沿墙传感器2实时将沿墙信号值ir发送至主控单元；

步骤2：主控单元对实时接收到的沿墙信号值ir与最大沿墙信号值max_ir进行对比分析，选取最大值作为最大沿墙信号值max_ir，并取最大沿墙信号值max_ir的60％-80％作为目标信号值；

步骤3：主控单元计算出实时接收的沿墙信号值ir与上一次接收的沿墙信号值last_ir的差值，记为err_ir，发送至pid稳定控制单元，并同步计算出实时接收的沿墙信号值ir与目标信号值的差值，记为trg_err_ir,发送至pid距离控制单元；

步骤4：pid稳定控制单元接收到err_ir后计算出稳定轮速差dv1，并反馈至主控单元，pid距离控制单元接收到trg_err_ir后计算出距离轮速差dv2并同步反馈至主控单元；

步骤5：主控单元根据反馈结果计算出轮速差dv，并根据轮速差dv控制左、右驱动轮的轮速，调整扫地机器人的行进姿态以及扫地机器人与被探测墙面之间的距离。

步骤6：扫地机器人行走期间，若碰撞传感器发送碰撞信号至主控单元，主控单元则执行内转程序或避障程序，否则保持直行。

所述pid稳定控制单元接收到err_ir以及pid距离控制单元接收到trg_err_ir后，均先通过公式cof＝kp×δsn+ki×∑δs+kd×(δsn-δsn-1)计算出速度差值cof，式中，kp为比例控制权重，ki为积分控制权重，kd为微分控制权重，δsn为本次输入值，δsn-1为上一次的输入值；

再通过公式进行限幅运算，得出稳定轮速差dv1或距离轮速差dv2，式中，maxspeeddif为最大可设置的速度差，max_ir为采集到的最大信号，maxintegrate为设定的积分限制。

所述主控单元接收到pid稳定控制单元发送的dv1以及pid距离控制单元发送的dv2后，对max_ir与设定的动态权重临界点进行对比分析：

若max_ir>动态权重临界点，主控单元根据公式dv＝dv1×a+dv2×b,a＝0.15,b＝0.85计算出轮速差dv，否则主控单元根据公式dv＝dv1×a+dv2×b,a＝0.9,b＝0.1计算出轮速差dv；

主控单元设置远离被探测墙面的驱动轮轮速为可设置的最大轮速，另一驱动轮轮速为远离被探测墙面的驱动轮轮速和轮速差dv的差值。

所述主控单元执行内转程序时，将max_ir重置为0，并驱动扫地机器人朝沿墙方式相同方向转动，并检测是否有碰撞，若有碰撞则执行避障程序，若无碰撞，则继续转动直至发生碰撞。

所述主控单元执行避障程序，将max_ir重置为0，驱动扫地机器人后退一定距离，朝沿墙方式相反方向转动，转动期间，主控单元对实时接收的沿墙信号值ir进行判断分析，若max_ir>ir≥min_ir,主控单元执行沿墙程序，否则驱动扫地机器人继续转动。

所述主控单元执行沿墙程序，将max_ir重置为0，并通过pid稳定控制单元和pid距离控制单元控制扫地机器人沿墙运动，并检测是否有碰撞，若有碰撞则执行避障程序，否则主控单元对实时接收的沿墙信号值ir进行分析判断，若ir<min_ir或(last_ir-ir)/last_ir<0.5或ir/last_ir<0.2，则执行内转程序，否则继续前进。

实施例1

步骤1：开启扫地机器人，设定最小沿墙信号值min_ir和最大沿墙信号值max_ir，开启扫地机器人，扫地机器人进行一次碰撞检测判断，若主控单元未接收碰撞信号，则进入初始状态，即沿当前方向向前直线行走，否则主控单元执行避障程序，初始状态等同于主控单元执行沿墙程序，驱动扫地机器人沿墙直线行走，沿墙传感器2实时(每一毫秒)将沿墙信号值ir发送至主控单元(若左沿墙则取左侧沿墙传感器信号值，若右沿墙则取右侧沿墙传感器信号值)；

步骤2：主控单元对实时接收到的沿墙信号值ir与最大沿墙信号值max_ir进行对比分析，选取两者中的最大值作为最大沿墙信号值max_ir实时更新，并取最大沿墙信号值max_ir的80％作为目标信号值；

步骤3：主控单元计算出实时接收的沿墙信号值ir与上一次接收的沿墙信号值last_ir的差值，记为err_ir，err_ir＝last_ir-ir,发送至pid稳定控制单元，并同步计算出实时接收的沿墙信号值ir与目标信号值的差值，记为trg_err_ir,trg_err_ir＝max_ir×80％-ir,发送至pid距离控制单元；

步骤4：pid稳定控制单元接收到err_ir后通过公式cof＝kp×δsn+ki×∑δs+kd×(δsn-δsn-1)计算出速度差值cof，式中，kp为比例控制权重(kp＝1)，ki为积分控制权重(ki＝0.9)，kd为微分控制权重(kd＝0.07)，δsn为本次输入值即err_ir，δsn-1为上一次的输入值即上一次输入的err_ir，再通过公式进行限幅运算(由于计算出的速度差值cof可能大于可设置的最大速度差)，得出稳定轮速差dv1，式中，maxspeeddif为最大可设置的速度差，max_ir为采集到的最大信号，maxintegrate为设定的积分限制，并反馈至主控单元，pid距离控制单元接收到trg_err_ir后通过公式cof＝kp×δsn+ki×∑δs+kd×(δsn-δsn-1)计算出速度差值cof，式中，kp为比例控制权重(kp＝0.6)，ki为积分控制权重(ki＝-0.01)，kd为微分控制权重(kd＝6)，δsn为本次输入值即trg_err_ir，δsn-1为上一次的输入值即上一次输入的trg_err_ir，再通过公式进行限幅运算(由于计算出的速度差值cof可能大于可设置的最大速度差)，得出稳定轮速差dv2并同步反馈至主控单元；

步骤5：主控单元接收反馈结果即pid稳定控制单元反馈的dv1和pid距离控制单元反馈的dv2后，对max_ir与设定的动态权重临界点(主控单元根据扫地机器人信号自动设定，动态权重临界点的取值范围为80-400，本实施例取160)进行对比分析，若max_ir>动态权重临界点，主控单元根据公式dv＝dv1×a+dv2×b,a＝0.15,b＝0.85计算出轮速差dv，否则主控单元根据公式dv＝dv1×a+dv2×b,a＝0.9,b＝0.1计算出轮速差dv，并根据轮速差dv控制左、右驱动轮的轮速即主控单元设置远离被探测墙面的驱动轮轮速(转向时位于外侧的驱动轮)为可设置的最大轮速，另一驱动轮(转向时位于内侧的驱动轮)轮速为远离被探测墙面的驱动轮轮速和轮速差dv的差值，调整扫地机器人的行进姿态(使得夹角的角平分线与被探测墙面垂直)以及扫地机器人与被探测墙面之间的距离不超过5mm。

步骤6：扫地机器人行走期间，若碰撞传感器发送碰撞信号至主控单元，主控单元则执行内转程序或避障程序，否则保持直行：

所述主控单元执行内转程序时，将max_ir重置为0，并驱动扫地机器人朝沿墙方式相同方向(左沿墙方式朝左转，右沿墙方式向右转)转动，并检测是否有碰撞，若有碰撞则执行避障程序，若无碰撞，则继续转动直至发生碰撞。

所述主控单元执行避障程序，将max_ir重置为0，驱动扫地机器人后退一定距离，朝沿墙方式相反方向转动(左沿墙方式朝右转，右沿墙方式朝左转)，转动期间，主控单元对实时接收的沿墙信号值ir进行判断分析，若max_ir>ir≥min_ir,主控单元执行沿墙程序，否则驱动扫地机器人继续转动。

所述主控单元执行沿墙程序，将max_ir重置为0，并通过pid稳定控制单元和pid距离控制单元控制扫地机器人沿墙运动，并检测是否有碰撞，若有碰撞则执行避障程序，否则主控单元对实时接收的沿墙信号值ir进行分析判断，若ir<min_ir或(last_ir-ir)/last_ir<0.5或ir/last_ir<0.2，则执行内转程序，否则继续前进。

扫地机器人在沿墙行走的过程中，其工作状态可概括为初始状态、避障状态、沿墙状态以及内转状态，且四种工作状态如上步骤进行转化(请参阅图4)，并且其一直处于运行之中。

由于扫地机器人离墙体越远，沿墙红外信号越弱，故err_i大于0则表明机器正在远离墙，反之err_ir小于0则表明扫地机器人正在靠近墙，故使用err_ir作为输入的pid稳定控制单元会使机器前进方向稳定于墙体平行线。由于目标沿墙信号为max_ir的80％，max_ir根据墙体的反射性能决定，trg_err_ir大于0则表明扫地机器人与被探测墙面之间的距离大于目标沿墙距离，小于0则表明扫地机器人过于接近被探测墙面，故使用trg_err_ir作为输入的pid距离控制单元会使扫地机器人离墙距离尽可能小，两个pid控制单元根据动态权重临界点切换权重，可以使机器在高反光的墙体和低反光的墙体两种不同墙体上选择控制偏重，使沿墙算法能适应各种墙体。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。