[发明专利]一种基于有限状态机的扫地机目标点导航方法

权利要求书

1.一种基于有限状态机的扫地机目标点导航方法，其特征在于：包括开始状态、扫地机转动朝向目标点状态、沿线控制状态、沿边控制状态以及到达目标点，结束状态；目标点导航初始化后，由开始状态转换到扫地机转动朝向目标点状态；经过朝向目标点的判断后，由扫地机转动朝向目标点状态转换到沿线控制状态；经过进入到沿边控制的判断，由沿线控制状态转换到沿边控制状态；经过由沿边控制退出到沿线控制的判断，由沿边控制状态转换到扫地机转动朝向目标点状态；经过沿线过程中偏离目标点的判断，由沿线控制状态转换到扫地机转动朝向目标点状态；经过到达目标点的判断，由沿线控制状态转换到到达目标点，结束状态；

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

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

S31、设扫地机当前行进方向为直线L0，L0经过逆时针旋转45°得到直线L1，直线L0经过顺时针旋转45°得到直线L2；

S32、求出目标点与直线L0、L1、L2的绝对距离，即可得到当前点与目标点的相对位置；

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

2.根据权利要求1所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：以扫地机为当前点，直线L1、L2将平面空间分为四块，并从直线L1右侧开始依次命名为第一区域、第二区域、第三区域、第四区域，若目标点位于第一区域内，则调用沿线PID模块进行转向控制，若目标点位于第四区域内，扫地机满速左转，若目标点位于第二区域内，则扫地机满速右转。

3.根据权利要求2所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：所述进入到沿边控制的判断包括碰撞检测和跌落检测，所述碰撞检测通过对位于扫地机前档的红外传感器组以及微动开关组反馈的数据进行识别，所述跌落检测通过扫地机底部的红外传感器组反馈的数据进行识别，并且进入沿边模式后的本次沿边方向始终与上一次沿边方向相反。

4.根据权利要求3所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：所述由沿边控制退出到沿线控制的判断包括沿边控制进入内转角或找墙5s后退出；沿边控制距离过长后退出；进入沿边控制的初始点所在目标线的方向和沿边过程中当前点所在目标线的方向相反时退出。

5.根据权利要求4所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：所述沿线过程中偏离目标点的判断为当扫地机行走一段时间后，目标点的相对位置位于第一区域和第二区域，则判断结果为扫地机偏离目标点。

6.根据权利要求5所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：所述到达目标点的判断包括：计算出当前点的横坐标与目标点的横坐标之差的绝对值、当前点的纵坐标与目标点的纵坐标之差的绝对值，并进行判断是否小于设定值，若是则判断扫地机到达目标点，结束目标点导航过程。

7.根据权利要求6所述的一种基于有限状态机的扫地机目标点导航方法，其特征在于：所述沿线PID模块包括偏距PID控制单元，包括如下步骤：

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

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

S93、偏距PID控制单元根据公式计算出速度增量dv，公式如下：cof＝K_p·Derr₂+K_i·∑Derr₂+K_d(Derr₂(k)-Derr₂(k-1))、式中，Derr表示目标点到目标直线的偏距，Derr₁表示目标点到当前点的距离，dV_max为扫地机的最大速度差，DistanceErr_max为偏距最大值，SumDistance_max为偏距最大累加值，K_p、K_i,K_d为偏距PID控制单元的积分、微分、比例系数；

S94、对速度增量dv进行判断，当dv0时，设置左轮轮速V_左＝V_max，右轮轮速V_右＝V_max-dv，当dv0时，设置左轮轮速V_左＝V_max-dv，右轮轮速V_右＝V_max，当dv＝0时，V_左＝V_右。