一种基于有限状态机的扫地机目标点导航方法与流程

本发明属于扫地机器人技术领域，具体涉及一种基于有限状态机的扫地机目标点导航方法。

背景技术：

当前市面上出售的扫地机器人上装载的目标点导航方法存在以下缺陷：需要建立环境地图，并且对扫地机的芯片要求较高；导航过程中无法考虑到动态障碍物，易受到环境干扰；导航过程中的姿态控制紊乱，无法实现精准控制，且导航到的点离目标点较远；导航过程中的各个状态无法实现统一管理，查找问题时难度大。

技术实现要素：

(一)解决的技术问题

针对现有技术的不足，本发明提供一种基于有限状态机的扫地机目标点导航方法，能够解决环境干扰、姿态控制紊乱、各个状态无法统一管理的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：一种基于有限状态机的扫地机目标点导航方法，包括开始状态、扫地机转动朝向目标点状态、沿线控制状态、沿边控制状态以及到达目标点，结束状态；目标点导航初始化后，由开始状态转换到扫地机转动朝向目标点状态；经过朝向目标点的判断后，由扫地机转动朝向目标点状态转换到沿线控制状态；经过进入到沿边控制的判断，由沿线控制状态转换到沿边控制状态；经过由沿边控制退出到沿线控制的判断，由沿边控制状态转换到扫地机转动朝向目标点状态；经过沿线过程中偏离目标点的判断，由沿线控制状态转换到扫地机转动朝向目标点状态；经过到达目标点的判断，由沿线控制状态转换到到达目标点，结束状态。

进一步的，所述目标点导航初始化包括开始点初始化，当前状态初始化和其他导航标志位初始化。

进一步的，所述朝向目标点的判断包括如下步骤：

s31、设扫地机当前行进方向为直线l0，l0经过逆时针旋转45°得到直线l1，直线l0经过顺时针旋转45°得到直线l2；

s32、求出目标点与直线l0、l1、l2的绝对距离，即可得到当前点与目标点的相对位置；

s33、扫地机根据当前点与目标点的相对位置进行方向调节。

进一步的，以扫地机为当前点，直线l1、l2将平面空间分为四块，并从直线l1右侧开始依次命名为第一区域、第二区域、第三区域、第四区域，若目标点位于第一区域内，则调用沿线pid模块进行转向控制，若目标点位于第四区域内，扫地机满速左转，若目标点位于第二区域内，则扫地机满速右转。

进一步的，所述进入到沿边控制的判断包括碰撞检测和跌落检测，所述碰撞检测通过对位于扫地机前档的红外传感器组以及微动开关组反馈的数据进行识别，所述跌落检测通过扫地机底部的红外传感器组反馈的数据进行识别，并且进入沿边模式后的本次沿边方向始终与上一次沿边方向相反。

进一步的，所述由沿边控制退出到沿线控制的判断包括沿边控制进入内转角或找墙5s后退出；沿边控制距离过长后退出；进入沿边控制的初始点所在目标线的方向和沿边过程中当前点所在目标线的方向相反时退出。

进一步的，所述沿线过程中偏离目标点的判断为当扫地机行走一段时间后，目标点的相对位置位于第一区域和第二区域，则判断结果为扫地机偏离目标点。

进一步的，所述到达目标点的判断包括：计算出当前点的横坐标与目标点的横坐标之差的绝对值、当前点的纵坐标与目标点的纵坐标之差的绝对值，并进行判断是否小于设定值，若是则判断扫地机到达目标点，结束目标点导航过程。

进一步的，所述沿线pid模块包括偏距pid控制单元，包括如下步骤：

s91、扫地机输出当前点坐标、目标点坐标以及期望偏距至偏距pid控制单元；

s92、偏距pid控制单元通过当前点和目标点的坐标计算出目标直线和当前点到直线的偏距d，d＝当前点到目标直线的距离-期望偏距；

s93、偏距pid控制单元根据公式计算出速度增量dv，公式如下：cof＝kp·derr2+ki·∑derr2+kd(derr2(k)-derr2(k-1))、式中，derr表示目标点到目标直线的偏距，derr1表示目标点到当前点的距离，dvmax为扫地机的最大速度差，distanceerrmax为偏距最大值，sumdistancemax为偏距最大累加值，kp、ki,kd为偏距pid控制单元的积分、微分、比例系数；

s94、对速度增量dv进行判断，当d＞0时，设置左轮轮速v左＝vmax，右轮轮速v右＝vmax-dv，当d<0时，设置左轮轮速v左＝vmax-dv，右轮轮速v右＝vmax，当d＝0时，v左＝v右。

(三)有益效果

本发明提供了一种基于有限状态机的扫地机目标点导航方法，具备以下益效果：本发明成本低，对内存依赖极小，可在stm芯片成本最低的stm32f0x0芯片中与其他机器人算法一同流畅运行；针对复杂环境下的情形，也能成功导航到目标点规定范围内；使用偏距pid控制算法实现沿线行走方式，控制姿态较为稳定；导航过程中的状态出现问题时，能快速找到问题。

附图说明

图1为本发明的目标点导航算法的fsm实现示意图；

图2为本发明的当前点与目标点的朝向示意图；

图3为本发明的偏距pid控制流程示意图。

具体实施方式

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

一种基于有限状态机的扫地机目标点导航方法，包括开始状态s1、扫地机转动朝向目标点状态s2、沿线控制状态s3、沿边控制状态s4以及到达目标点，结束状态s5；目标点导航初始化f1后，由开始状态s1转换到扫地机转动朝向目标点状态s2；经过朝向目标点的判断f2后，由扫地机转动朝向目标点状态s2转换到沿线控制状态s3；经过进入到沿边控制的判断f3，由沿线控制状态s3转换到沿边控制状态s4；经过由沿边控制退出到沿线控制的判断f4，由沿边控制状态s4转换到扫地机转动朝向目标点状态s2；经过沿线过程中偏离目标点的判断f5，由沿线控制状态s3转换到扫地机转动朝向目标点状态s2；经过到达目标点的判断f6，由沿线控制状态s3转换到到达目标点s5，结束状态。见图1所示的5种状态fsm示意图。

所述目标点导航初始化包括开始点初始化，当前状态初始化和其他导航标志位初始化。

所述朝向目标点的判断包括如下步骤：

s31、设扫地机当前行进方向为直线l0的正方向，l0经过逆时针旋转45°得到直线l1，直线l0经过顺时针旋转45°得到直线l2，目标点可能的出现点有三种，在直线l1与直线l2之间的范围点p1，直线l1的左边范围点p3，在直线l2的右边范围点p2；

s32、求出目标点与直线l0、l1、l2的绝对距离，即可得到当前点与目标点的相对位置；

s33、扫地机根据当前点与目标点的相对位置进行方向调节。

以扫地机为当前点，直线l1、l2将平面空间分为四块，并从直线l1右侧开始依次命名为第一区域、第二区域、第三区域、第四区域，若目标点位于第一区域内即p1(或第三区域)，则调用沿线pid模块进行转向(姿态)控制，若目标点位于第四区域内即怕，扫地机满速左转，若目标点位于第二区域内即p2，则扫地机满速右转。

所述进入到沿边控制的判断包括碰撞检测和跌落检测，所述碰撞检测通过对位于扫地机前档的红外传感器组(三个红外传感器组成的红外传感器组)以及微动开关组(两个微动开关组成的微动开关组)反馈的数据进行识别，所述跌落检测通过扫地机底部的红外传感器组(四个红外传感器组成的红外传感器组)反馈的数据进行识别，并且进入沿边模式后的本次沿边方向始终与上一次沿边方向相反。

所述由沿边控制退出到沿线控制的判断包括沿边控制进入内转角或找墙5s后退出；沿边控制距离过长后退出；进入沿边控制的初始点所在目标线的方向和沿边过程中当前点所在目标线的方向相反时退出。

在沿线过程中，扫地机可能由于轮子打滑或外界因素导致偏离目标点，此时根据图1所示的位置关系判断，所述沿线过程中偏离目标点的判断为当扫地机行走一段时间后，目标点的相对位置位于第一区域和第二区域，则判断结果为扫地机偏离目标点。

所述到达目标点的判断包括：计算出当前点的横坐标与目标点的横坐标之差的绝对值、当前点的纵坐标与目标点的纵坐标之差的绝对值(横纵坐标由扫地机测得目标点后，以自身当前点为原点建立的虚拟坐标系)，并进行判断是否小于设定值，设定值很小，根据扫地机精度来确定，若是则判断扫地机到达目标点，结束目标点导航过程。

所述沿线pid模块包括偏距pid控制单元，包括如下步骤：

s91、扫地机输出当前点坐标、目标点坐标以及期望偏距至偏距pid控制单元；

s92、偏距pid控制单元通过当前点和目标点的坐标计算出目标直线和当前点到直线的偏距d，d＝当前点到目标直线的距离-期望偏距；

s93、偏距pid控制单元根据公式计算出速度增量dv，公式如下：cof＝kp·derr2+ki·∑derr2+kd(derr2(k)-derr2(k-1))、式中，derr表示目标点到目标直线的偏距，derr1表示目标点到当前点的距离，dvmax为扫地机的最大速度差，distanceerrmax为偏距最大值，sumdistancemax为偏距最大累加值，kp、ki,kd为偏距pid控制单元的积分、微分、比例系数；

s94、对速度增量dv进行判断，采用缩放映幅的方式将偏距pid控制单元输出融合到扫地机左右两轮的轮速中，当d＞0时，扫地机向左偏离目标线，应右转，设置左轮轮速v左＝vmax，右轮轮速v右＝vmax-dv，当d<0时，扫地机向右偏离目标线，应左转，设置左轮轮速v左＝vmax-dv，右轮轮速v右＝vmax，当d＝0时，扫地机在目标线上，v左＝v右。

本实施例旨在解决无地图的扫地机目标点导航问题，可用于寻找回充站或者清扫完成后回到起始点。基于有限状态机的导航方法大大缩减了寻找回充站的时间，并且导航过程中的各个状态实现了统一管理

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。