CN110840344A - 一种基于pid控制算法的扫地机器人沿线行走控制系统及其控制方法 - Google Patents

Abstract

本发明公开了一种基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法，包括扫地机机体、左转动轮、右转动轮，底层驱动模块、控制模块以及电源模块，所述底层驱动模块包括扫地机机体姿态测量单元以及驱动单元，本发明提出了基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法，对扫地机器人在行进过程中的姿态角、速度和位置(即偏距)进行了闭环控制，使得扫地机器人在行进过程中保持直线，并且适用于各种机型的扫地机器人，调参方便，控制稳定，在低成本下完成了扫地机器人的智能控制。

Description

一种基于PID控制算法的扫地机器人沿线行走控制系统及其 控制方法

技术领域

本发明涉及扫地机器人技术领域，具体为一种基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法。

背景技术

人们通常将完成清扫、吸尘、擦地工作的机器人，也统称为智能扫地机器人，智能扫地机器人能够凭借一定的人工智能技术，自主在房间内完成清扫任务，一般采用刷扫和真空方式，将地面杂物先吸纳进入自身的垃圾收纳盒，从而完成地面清扫的功能。

目前市场上的智能扫地机器人还没有一种标准的直线行走控制方法，也没有通过自动控制算法来实现对扫地机器人的姿态、速度和位置控制。虽然市面上已经出现了对扫地机器人的测速、测距及转角控制，但都是独立的模块单独开环控制，还没有出现将其结合后基于PID算法的扫地机机器人沿线行走的案例。

公开号为CN201410266593.6的专利提供了一种扫地机器人测速、测距系统及其方法，通过在万向轮上设置多级磁环，在底盘上对应设置霍尔感应器，可以较为准确的测量出扫地机器人当前的速度，但是对于扫地机器人的速度未有闭环控制。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法，解决了现有技术不能平缓的控制扫地机器人行走时的速度以及扫地机器人在直线行走时偏移较大的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：一种基于PID控制算法的扫地机器人沿线行走控制系统，包括扫地机机体、左转动轮、右转动轮、底层驱动模块、控制模块以及电源模块，所述底层驱动模块包括扫地机机体姿态测量单元以及驱动单元，所述机体姿态测量单元设置于所述扫地机机体内，所述驱动单元用于驱动左、右转动轮带动所述扫地机机体移动，所述左、右转动轮上均对应设置有用于测量左、右转动轮行进过程中脉冲数的左、右编码单元，所述控制模块为PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元均电性连接所述PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元、PID控制模块均电性连接所述电源模块。

优选的，所述PID控制模块包括主控单元、姿态环PID控制单元、速度环PID控制单元和偏距环PID控制单元，所述机体姿态测量单元电性连接所述姿态环PID控制单元，所述左、右编码单元均电性连接所述速度环PID控制单元，所述姿态环PID控制单元与所述速度环PID控制单元电性连接所述偏距环PID控制单元，所述姿态环PID控制单元、所述速度环PID控制单元、所述偏距环PID控制单元均设置于所述主控单元上。

优选的，所述姿态环PID控制单元以及所述偏距环PID控制单元采用位置式PID控制算法，所述速度环PID采用增量式PID控制算法。

优选的，所述机体姿态测量单元为陀螺仪，所述左、右编码单元为左、右光电编码器，所述驱动单元包括左直流有刷电机、右直流有刷电机，所述左、右光电编码器对应设置于左、右直流有刷电机上。

此外，为实现上述目的，本发明还提供一种基于PID控制算法的扫地机器人沿线行走的控制方法，包括如上所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，所述扫地机机体内还设置有定时单元，所述定时单元电性连接所述左光电编码器、右光电编码器以及PID控制模块：

步骤1：开启扫地机器人；

步骤2：所述扫地机器人系统初始化，清除所述陀螺仪、左光电编码器、右光电编码器内存储的采集数据；

步骤3：所述扫地机器人发送起始信号至定时单元，若定时单元未向主控单元发送中断信号，则所述姿态环PID控制单元扫描陀螺仪采集的数据，且所述速度环PID控制单元同步扫描左、右光电编码器扫描的数据，否则执行步骤2；

步骤4：所述姿态环PID控制单元和所述速度环PID控制单元分别将扫描数据与设定值分析对比，并同步将对比值发送至主控单元和偏距环PID控制单元；

步骤5：所述偏距环PID控制单元对接收自姿态环PID控制单元和速度环 PID控制单元的对比值进行计算并与设定值进行对比分析后将对比值发生制主控单元，所述主控单元对接收到的对比值进行综合分析，并根据分析值控制左、右转动轮运动。

优选的，所述姿态环PID控制单元将扫描自陀螺仪的实时机体姿态角与设定期望姿态角进行对比分析，计算出姿态环PWM占空比值，所述速度环PID 控制单元根据扫描自左、右光电编码器的单位采样时间内的实时脉冲计数得出实时左、右转动轮速度值，并将实时左、右转动轮速度值与设定期望速度值进行对比分析，计算出左、右转动轮速度环PWM占空比值，所述姿态环PID 控制单元与所述速度环PID控制单元均同步将计算值发送至偏距环PID控制单元和主控单元；

所述偏距环PID控制单元根据接收的姿态环PWM占空比值和左、右转动轮速度环PWM占空比值计算出当前扫地机器人的实时偏距，且根据实时偏距与设定期望偏距进行对比分析，得出偏距环PWM占空比值，并将偏距环PWM 占空比值发送至主控单元；

所述主控单元对接收到的姿态环PWM占空比值、速度环PWM占空比值以及偏距环PWM占空比值进行综合计算分析，得出左转动轮PWM占空比和右转动轮PWM占空比，并根据左、右转动轮PWM占空比对应发送至所述左、右直流有刷电机控制扫地机器人的行进轨迹。

优选的，所述主控单元综合计算公式为左转动轮PWM占空比＝左转动轮速度环PWM占空比+偏距环PWM占空比+0.5姿态环PWM占空比右转动轮PWM占空比＝右转动轮速度环PWM占空比-偏距环PWM占空比+0.5姿态环PWM占空比。

优选的，其特征在于，所述陀螺仪采集的机体姿态角范围为-180°～ +180°，且设定期望机体姿态角范围为-180°～+180°，所述左、右光电编码器的单位采样时间为20ms，所述设定期望偏距值为0mm。

优选的，当所述扫地机器人偏离期望轨迹角度大于10°时，主控单元不进行轨迹控制。

优选的，所述左、右光电编码器采集的脉冲计数经过一次低通滤波后发送至所述速度环PID控制单元。

(三)有益效果

本发明提供了一种基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法。具备以下益效果：本发明提出了基于PID控制算法的扫地机器人沿线行走控制系统及其控制方法，对扫地机器人在行进过程中的姿态角、速度和位置(即偏距)进行了闭环控制，使得扫地机器人在行进过程中保持直线，并且适用于各种机型的扫地机器人，调参方便，控制稳定，在低成本下完成了扫地机器人的智能控制。

附图说明

图1为本发明的硬件结构程序示意框图；

图2为本发明的沿线控制程序示意框图；

图3为本发明的直流有刷电机控制的主程序流程示意图。

具体实施方式

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

请参阅图1-3，本发明实施例提供一种技术方案，一种基于PID控制算法的扫地机器人沿线行走控制系统，包括扫地机机体、左转动轮、右转动轮、底层驱动模块、控制模块以及电源模块，所述底层驱动模块包括扫地机机体姿态测量单元以及驱动单元，所述机体姿态测量单元设置于所述扫地机机体内，所述驱动单元用于驱动左、右转动轮带动所述扫地机机体移动，所述左、右转动轮上均对应设置有用于测量左、右转动轮行进过程中脉冲数的左、右编码单元，所述控制模块为PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元均电性连接所述PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元、PID控制模块均电性连接所述电源模块，电源模块给，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元、PID控制模块供电。

所述PID控制模块包括主控单元、姿态环PID控制单元、速度环PID控制单元和偏距环PID控制单元，所述机体姿态测量单元电性连接所述姿态环 PID控制单元，所述左、右编码单元均电性连接所述速度环PID控制单元，所述姿态环PID控制单元与所述速度环PID控制单元电性连接所述偏距环PID 控制单元，所述姿态环PID控制单元、所述速度环PID控制单元、所述偏距环PID控制单元均设置于所述主控单元上，即姿态环PID控制单元、、速度环PID控制单元、偏距环PID控制单元均电性连接主控单元。

所述姿态环PID控制单元以及所述偏距环PID控制单元采用位置式PID 控制算法，所述速度环PID采用增量式PID控制算法。

所述机体姿态测量单元为陀螺仪，所述左、右编码单元为左、右光电编码器，所述驱动单元包括左直流有刷电机、右直流有刷电机，所述左、右光电编码器对应设置于左、右直流有刷电机上。

一种基于PID控制算法的扫地机器人沿线行走的控制方法，包括如上所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，所述扫地机机体内还设置有定时单元，所述定时单元电性连接所述左光电编码器、右光电编码器以及PID控制模块：

步骤1：开启扫地机器人；

步骤2：所述扫地机器人系统初始化，清除所述陀螺仪、左光电编码器、右光电编码器内存储的采集数据，陀螺仪采集的数据、左光电编码器、右光电编码器内采集的脉冲计数只在扫地机器人开机时清除重置，即扫地机器人开机后，定时单元向主控单元发送中断信号后执行步骤2时，陀螺仪、左光电编码器、右光电编码器内采集的数据不进行清除重置；

步骤3：所述扫地机器人发送起始信号至定时单元，若定时单元未向主控单元发送中断信号，则所述姿态环PID控制单元扫描陀螺仪采集的数据，且所述速度环PID控制单元同步扫描左、右光电编码器扫描的数据，否则执行步骤2；

步骤4：所述姿态环PID控制单元和所述速度环PID控制单元分别将扫描数据与设定值分析对比，并同步将对比值发送至主控单元和偏距环PID控制单元；

步骤5：所述偏距环PID控制单元对接收自姿态环PID控制单元和速度环 PID控制单元的对比值进行计算并与设定值进行对比分析后将对比值发生制主控单元，所述主控单元对接收到的对比值进行综合分析，并根据分析值控制左、右转动轮运动。

所述姿态环PID控制单元将扫描自陀螺仪的实时机体姿态角与设定期望姿态角进行对比分析，计算出姿态环PWM占空比值，所述速度环PID控制单元根据扫描自左、右光电编码器的单位采样时间内的实时脉冲计数得出实时左、右转动轮速度值，并将实时左、右转动轮速度值与设定期望速度值进行对比分析，计算出左、右转动轮速度环PWM占空比值(左、右光电编码器分别读取20ms内左、右转动轮的脉冲计数，经过码盘计算与速度的转换公式(该转换公式可为本领域人员常用技术，也可为本申请人研究出的成果)得到当前扫地机器人的整体速度，该整体速度为左、右转动轮的平均值)，所述姿态环PID控制单元与所述速度环PID控制单元均同步将计算值发送至偏距环 PID控制单元和主控单元；

所述偏距环PID控制单元根据接收的姿态环PWM占空比值和左、右转动轮速度环PWM占空比值计算出当前扫地机器人的实时偏距，且根据实时偏距与设定期望偏距进行对比分析，若扫地机器人往右偏则偏距为正，往左偏则偏距为负，得出偏距环PWM占空比值，并将偏距环PWM占空比值发送至主控单元；

所述主控单元对接收到的姿态环PWM占空比值、速度环PWM占空比值以及偏距环PWM占空比值进行综合计算分析，得出左转动轮PWM占空比和右转动轮PWM占空比，并根据左、右转动轮PWM占空比对应发送至所述左、右直流有刷电机控制扫地机器人的行进轨迹。

所述主控单元综合计算公式为左转动轮PWM占空比＝左转动轮速度环PWM占空比+偏距环PWM占空比+0.5姿态环PWM占空比右转动轮PWM占空比＝右转动轮速度环PWM占空比-偏距环PWM占空比+0.5姿态环PWM占空比。

其特征在于，所述陀螺仪采集的机体姿态角范围为-180°～+180°，且设定期望机体姿态角范围为-180°～+180°，根据陀螺仪的读数，判断扫地机机器人顺时针转动时角度为正，逆时针转动时角度为负，所述左、右光电编码器的单位采样时间为20ms，所述设定期望偏距值为0mm，设定值均可自由调参设置。

当所述扫地机器人偏离期望轨迹角度大于10°时，主控单元不进行轨迹控制，若扫地机器人在行进过程中往右偏，通过PID控制模块可控制扫地机器人往左转回到预期轨迹上，若扫地机器人在行进过程中往左偏，通过PID 模块器可控制机器人往右转回到预期轨迹上；

在扫地机器人直行行走过程中若受到较大地外部干扰，使其偏离轨迹的角度大于10°，则不进行PID算法控制。具体地来说，若当前陀螺仪的读数大于﹢10°，则左直流有刷电机驱动左转动轮满速反转，右直流有刷电机驱动右转动轮满速正转；若当前陀螺仪的读数小于-10°，则左直流有刷电机驱动左转动轮满速正转，右直流有刷电机驱动右转动轮满速反转，能够使扫地机器人受到较严重的干扰后，迅速回到原来的预期轨迹上。。

所述左、右光电编码器采集的脉冲计数经过一次低通滤波后发送至所述速度环PID控制单元，左、右光电编码器采集的脉冲计数只需要经过一次低通滤波便可以稳定计算出扫地机器人当前的速度，而陀螺仪采集的机体姿态角的处理需要经过校验保证数据输出的完整性与正确性。

本发明采用PID算法控制后，扫地机器人速度平缓，稳定直行在预期直行轨迹上，同时可避免因转动轮打滑等外壁因素导致系统失去平衡。

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

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

Claims (10)

1.一种基于PID控制算法的扫地机器人沿线行走控制系统，包括扫地机机体、左转动轮、右转动轮，其特征在于，包括底层驱动模块、控制模块以及电源模块，所述底层驱动模块包括扫地机机体姿态测量单元以及驱动单元，所述机体姿态测量单元设置于所述扫地机机体内，所述驱动单元用于驱动左、右转动轮带动所述扫地机机体移动，所述左、右转动轮上均对应设置有用于测量左、右转动轮行进过程中脉冲数的左、右编码单元，所述控制模块为PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元均电性连接所述PID控制模块，所述机体姿态测量单元、驱动单元、左编码单元、右编码单元、PID控制模块均电性连接所述电源模块。

2.根据权利要求1所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，其特征在于，所述PID控制模块包括主控单元、姿态环PID控制单元、速度环PID控制单元和偏距环PID控制单元，所述机体姿态测量单元电性连接所述姿态环PID控制单元，所述左、右编码单元均电性连接所述速度环PID控制单元，所述姿态环PID控制单元与所述速度环PID控制单元电性连接所述偏距环PID控制单元，所述姿态环PID控制单元、所述速度环PID控制单元、所述偏距环PID控制单元均设置于所述主控单元上。

3.根据权利要求2所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，其特征在于，所述姿态环PID控制单元以及所述偏距环PID控制单元采用位置式PID控制算法，所述速度环PID采用增量式PID控制算法。

4.根据权利要求1所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，其特征在于，所述机体姿态测量单元为陀螺仪，所述左、右编码单元为左、右光电编码器，所述驱动单元包括左直流有刷电机、右直流有刷电机，所述左、右光电编码器对应设置于左、右直流有刷电机上。

5.一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，包括如权利要求1～4任意一项所述的一种基于PID控制算法的扫地机器人沿线行走控制系统，所述扫地机机体内还设置有定时单元，所述定时单元电性连接所述左光电编码器、右光电编码器以及PID控制模块：

步骤1：开启扫地机器人；

步骤2：所述扫地机器人系统初始化，清除所述陀螺仪、左光电编码器、右光电编码器内存储的采集数据；

步骤3：所述扫地机器人发送起始信号至定时单元，若定时单元未向主控单元发送中断信号，则所述姿态环PID控制单元扫描陀螺仪采集的数据，且所述速度环PID控制单元同步扫描左、右光电编码器扫描的数据，否则执行步骤2；

步骤4：所述姿态环PID控制单元和所述速度环PID控制单元分别将扫描数据与设定值分析对比，并同步将对比值发送至主控单元和偏距环PID控制单元；

步骤5：所述偏距环PID控制单元对接收自姿态环PID控制单元和速度环PID控制单元的对比值进行计算并与设定值进行对比分析后将对比值发生制主控单元，所述主控单元对接收到的对比值进行综合分析，并根据分析值控制左、右转动轮运动。

6.根据权利要求5所述的一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，所述姿态环PID控制单元将扫描自陀螺仪的实时机体姿态角与设定期望姿态角进行对比分析，计算出姿态环PWM占空比值，所述速度环PID控制单元根据扫描自左、右光电编码器的单位采样时间内的实时脉冲计数得出实时左、右转动轮速度值，并将实时左、右转动轮速度值与设定期望速度值进行对比分析，计算出左、右转动轮速度环PWM占空比值，所述姿态环PID控制单元与所述速度环PID控制单元均同步将计算值发送至偏距环PID控制单元和主控单元；

所述偏距环PID控制单元根据接收的姿态环PWM占空比值和左、右转动轮速度环PWM占空比值计算出当前扫地机器人的实时偏距，且根据实时偏距与设定期望偏距进行对比分析，得出偏距环PWM占空比值，并将偏距环PWM占空比值发送至主控单元；

所述主控单元对接收到的姿态环PWM占空比值、速度环PWM占空比值以及偏距环PWM占空比值进行综合计算分析，得出左转动轮PWM占空比和右转动轮PWM占空比，并根据左、右转动轮PWM占空比对应发送至所述左、右直流有刷电机控制扫地机器人的行进轨迹。

7.根据权利要求6所述的一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，所述主控单元综合计算公式为

左转动轮PWM占空比＝左转动轮速度环PWM占空比+偏距环PWM占空比+0.5姿态环PWM占空比

右转动轮PWM占空比＝右转动轮速度环PWM占空比-偏距环PWM占空比+0.5姿态环PWM占空比。

8.根据权利要求5所述的一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，所述陀螺仪采集的机体姿态角范围为-180°～+180°，且设定期望机体姿态角范围为-180°～+180°，所述左、右光电编码器的单位采样时间为20ms，所述设定期望偏距值为0mm。

9.根据权利要求5所述的一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，当所述扫地机器人偏离期望轨迹角度大于10°时，主控单元不进行轨迹控制。

10.根据权利要求5所述的一种基于PID控制算法的扫地机器人沿线行走的控制方法，其特征在于，所述左、右光电编码器采集的脉冲计数经过一次低通滤波后发送至所述速度环PID控制单元。

Images