基于有限状态自动机架构的采摘机器人控制器及架构方法与流程

本发明属于机器人控制技术领域，特别涉及一种基于有限状态自动机架构的采摘机器人控制器及架构方法。

背景技术：

采摘机器人是21世纪精准农业的重要装备之一，是未来智能农业机械的发展方向，采摘机器人由机械手、末端执行器、移动机构、视觉系统以及控制系统等构成，其中控制系统解决采摘机器人的目标定位以及目标采摘，是整个机器人系统的核心和关键。日本在农业机器人研究方面走在世界的前列，目前已成功研制了西瓜采摘机器人，橙子采摘机器人，嫁接机器人等。美国研制了太阳能除草机器人，此外，我国在农业机器人的研究上也有一定的发展，上海交通大学机器人研究所已完成了智能化联合收割机等。

现有采摘机器人控制系统包括pc控制，还有基于神经网络进行机器人的控制，但是现有技术存在的机器人的控制存在以下缺陷：现有的控制系统通用性差，可扩展性不高，无法快速准确的采摘下目标。

技术实现要素：

针对上述问题，本发明提供一种新的基于有限状态自动机架构的采摘机器人控制器及架构方法，该基于有限状态自动机架构的采摘机器人控制器及架构方法通用性强，可扩展性高，并且通过状态的转换可以快速准确的采摘下目标。

本发明具体技术方案如下：

本发明提供一种基于有限状态自动机架构的采摘机器人控制器，所述控制器包括：

初始化模块：用于对系统进行初始化，并判断初始化是否完成，且初始化包含两种状态，即初态初始化s1和次态初始化s1’；

扫描模式模块：用于在系统初始化完成后，对采摘目标进行扫描，并判断是否有目标存在，且扫描模式包含两种状态，即初态扫描模式s2和次态扫描模式s2’；

对准模式模块：用于基于有目标存在时，控制采摘点接近采摘目标，并判断是否触及目标，且对准模式包含两种状态，即初态对准模式s3和次态对准模式s3’；

抓取模式模块：用于基于采摘点触及采摘目标时，控制采摘点剪下目标，抓取模式包含两种状态，即初态抓取模式s4和次态抓取模式s4'；

回归模式模块：用于进入回归模式，控制各轴回归记录点，回归模式包含两种状态，即初态回归模式s5和次态抓取回归s5’。

进一步的改进，所述初始化模块、所述扫描模式模块、对准模式模块、所述抓取模式模块以及回归模式模块根据状态转移公式进行模式的切换：

状态转移公式为：

其中，e表示条件，a表示动作，e1表示初始化完成，e2表示无目标，e3表示帧内有目标，e4表示目标存在采摘误差，e5表示触及目标，e6表示目标丢失，e7表示目标已被剪下，e8表示已经回归记录点，e9表示出现目标，e10表示未回归记录点，a1表示发送获取目标帧，a2表示保存当前位置，a3表示对比计算误差并控制采摘点接近目标，a4表示控制剪下目标，a5表示回归记录点，a6表示回到记录位置，a7表示回归记录点。

进一步的改进，所述初始化模块包括总初始化模块和找零模式模块，所述总初始化模块包括：

通讯初始化模块：用于与上位机通讯，且进行上位机的通讯模块初始化，并判断系统初始化是否完成；

返回模块：用于基于系统初始化未完成时，返回通讯初始化模块；

管理模块：用于基于系统初始化完成时，进行全局变量的初始化，在全局变量初始化完成后，向找零模式模块发送指令。

进一步的改进，所述找零模式模块包括：

第一判断模块：用于实时检测是否有x轴、y轴或z轴的零点标志产生，如果均有，向第一跳转模块发送指令，如果有一个轴没有，向开关状态判断模块发送指令；

开关状态判断模块：用于判断是否接收到x轴、y轴或z轴对应的开关信号，如果接收到开关信号，向第一设置模块发送指令，如果未接收到开关信号，向反零运动标志判断模块发送指令；

第一设置模块：用于生成x轴、y轴或z轴的反零运动标志，并向x轴、y轴或z轴发送反零方向运动的命令，同时设置x轴、y轴或z轴的反零运动单位脉冲，再向开关状态判断模块发送指令；

反零运动标志判断模块：用于判断x轴、y轴或z轴反零运动标志是否产生，如果反零运动标志产生，向第三设置模块发送指令，如果反零运动标志未产生，向第四设置模块发送指令；

第四设置模块：用于向x轴、y轴或z轴发送向零方向运动的命令，设置x轴、y轴或z轴的向零运动单位脉冲，并向第一判断模块发送指令；

第三设置模块：用于向x轴、y轴或z轴发送停止运动的指令；获取x轴、y轴或z轴电机的当前脉冲点，当前脉冲点为x轴、y轴或z轴的零点值，x轴、y轴或z轴零点标志产生，并向第一判断模块发送指令；

第一跳转模块：用于向扫描模式模块发送指令。

进一步的改进，扫描模式模块包括：

第一获取模块：用于获取z轴的零点值以及z轴的运动信息、目标存在状态信息，所述运动信息包括z轴运动的最低点和最高点，所述目标存在状态信息包括目标存在标志、目标位置坐标；

第二判断模块：用于判断是否存在目标存在标志，如果存在，向第二跳转模块发送指令，如果不存在，向第一管理模块发送指令；

第二跳转模块：用于记录当前时刻z轴位置，并产生目标存在标志，向对准模式模块发送指令；

第一管理模块：用于获取z轴相对位置，设置其为扫描格度值，并设置z轴在单位周期内走过的距离；

第三判断模块：用于根据扫描格度值与z轴的零点值判断z轴运动方向，当为向零运动时，向第一计算模块发送指令，当为反零运动时，向第二计算模块发送指令；

第一计算模块：用于计算扫描格度值与z轴在单位周期内走过的距离的差值；

第五判断模块：用于根据第一计算模块所得的差值判断是否到达z轴运动的最低点，如果到达z轴运动的最低点，向第二管理模块发送指令，如果未到达z轴运动的最低点，向第一获取模块发送指令；

第二管理模块：用于设置z轴的运行方向为反零方向，并向第一获取模块发送指令；

第二计算模块：用于计算扫描格度值与z轴在单位周期内走过的距离的和值；

第五判断模块：用于根据第二计算模块所得的和值判断是否到达z轴运动的最高点，如果到达z轴运动的最高点，向第三管理模块发送指令，如果未到达z轴运动的最高点，向第一获取模块发送指令；

第三管理模块：用于设置z轴的运行方向为向零方向，并向第一获取模块发送指令。

进一步的改进，对准模式模块包括：

第四设置模块：用于设置x轴误差阈值和z轴误差阈值；

第二获取模块：用于获取目标存在状态信息以及x轴、y轴和z轴的最大行程值，所述目标存在状态信息包括目标存在标志、目标位置坐标，计算目标位置坐标和第二跳转模块确定的位置坐标之间的x差值和z差值；

第六判断模块：用于判断目标存在标志是否产生，如果目标存在标志未产生，向回归模式模块发送指令，如果目标存在标志产生，向第七判断模块发送指令；

第七判断模块：用于判断x轴、y轴和z轴是否均到达对应轴的最大行程值，如果是，向回归模式模块发送指令，如果不是，向第八判断模块发送指令；

第八判断模块：用于判断x差值、z差值是否小于对应的x轴误差阈值、z轴误差阈值，如果有一差值大于误差阈值，向第四管理模块发送指令，如果x差值、z差值均小于对应的x轴误差阈值、z轴误差阈值，向第五管理模块发送指令；

第四管理模块：用于根据x差值或z差值计算对应轴偏差脉冲数，向x轴或z轴发送以x轴偏差脉冲数或z轴偏差脉冲数运行的指令，并向第二获取模块发送指令；

第五管理模块：用于向x轴或z轴发送停止运动的指令，同时设置y轴偏差脉冲数，并向y轴发送以y轴偏差脉冲数运行的指令；

第六管理模块：用于判断是否接收到触及目标的信号，如果接收到触及目标的信号，向抓取模式模块发送指令，如果未接收到触及目标的信号，向第二获取模块发送指令。

进一步的改进，抓取模式模块包括：

第一控制模块：用于向剪刀的电磁阀发送导通指令；

计时模块：用于在电磁阀导通时开始计时；

第二控制模块：用于在计时模块记录到时间到达预定时间时，向剪刀的电磁阀发送关闭指令，并跳转到回归模式模块。

进一步的改进，回归模式模块包括：

第六设置模块：用于设置x轴的行程中点为记录点，y轴零点值为记录点，第二跳转模块记录的z轴位置为z轴的记录点，并设置x轴、y轴、z轴的距离误差阈值；

第三获取模块：用于获取目标存在标志，并在目标存在标志不存在时，获取x轴、y轴、z轴的位置缓冲值及记录点，位置缓冲值为当前位置与记录点之间的行程值，并计算回归后的位置与记录点之间的x距离误差值、y距离误差值、z距离误差值；

回归模块：用于基于x轴、y轴、z轴的位置缓冲值，分别向x轴、y轴、z轴发送回归记录点的指令；

第十判断模块：用于判断x距离误差值、y距离误差值、z距离误差值是否小于对应的x轴的距离误差阈值、y轴的距离误差阈值、z轴的距离误差阈值，如果是，向扫描模式模块发送指令，如果有一距离误差值不小于距离误差阈值，向第七管理模块发送指令；

第七管理模块：用于设置x轴、y轴或z轴的回归速度脉冲数，向x轴、y轴或z轴发送以回归速度脉冲数运行的指令，并向第三获取模块发送指令。

一种基于有限状态自动机架构的架构方法，所述架构方法包括如下步骤：

s1：通过初始化模块对系统进行初始化，并判断初始化是否完成；

s2：通过扫描模式模块在系统初始化完成后，对采摘目标进行扫描，并判断是否有目标存在；

s3：通过对准模式模块基于有目标存在时，控制采摘点接近采摘目标，并判断是否触及目标；

s4：通过抓取模式模块基于采摘点触及采摘目标时，控制采摘点剪下目标；

s5：通过回归模式模块进入回归模式，控制各轴回归记录点。

本发明的有益效果如下：

本发明提供一种新的基于有限状态自动机架构的采摘机器人控制器及架构方法，该采摘机器人控制器及架构方法基于有限状态自动机实现对目标的快速采摘，工作时，首先对系统进行初始化，接着对目标进行扫描、对准、抓取，并在抓取完成后控制各轴回归到记录点；该设置根据当前的模式以及相应的条件和动作来进行扫描、对准、抓取、回归模式之间的切换，通过这种模式的转换可以快速准确的采摘下目标，使得通用性强，可扩展性高。

附图说明

图1为实施例1基于有限状态自动机架构的采摘机器人控制器的

结构框图；

图2为实施例3初始化模块的结构框图；

图3为实施例3找零模式模块的结构框图；

图4为实施例3扫描模式模块的结构框图；

图5为实施例3对准模式理模块的结构框图；

图6为实施例3抓取模式模块的结构框图；

图7为实施例3回归模式模块的结构框图；

图8为实施例4基于有限状态自动机架构的架构方法的流程图。

具体实施方式

下面结合附图和以下实施例对本发明作进一步详细说明。

实施例1

本发明实施例1提供一种基于有限状态自动机架构的采摘机器人控制器，如图1所示，所述控制器包括：

初始化模块1：用于对系统进行初始化，并判断初始化是否完成，且初始化包含两种状态，即初态初始化s1和次态初始化s1’；

扫描模式模块2：用于在系统初始化完成后，对采摘目标进行扫描，并判断是否有目标存在，且扫描模式包含两种状态，即初态扫描模式s2和次态扫描模式s2’；

对准模式模块3：用于基于有目标存在时，控制采摘点接近采摘目标，并判断是否触及目标，且对准模式包含两种状态，即初态对准模式s3和次态对准模式s3’；

抓取模式模块4：用于基于采摘点触及采摘目标时，控制采摘点剪下目标，抓取模式包含两种状态，即初态抓取模式s4和次态抓取模式s4’；

回归模式模块5：用于进入回归模式，控制各轴回归记录点，回归模式包含两种状态，即初态回归模式s5和次态抓取回归s5’。

本发明提供一种新的基于有限状态自动机架构的采摘机器人控制器，该采摘机器人控制器基于有限状态自动机实现对目标的快速采摘，工作时，首先对系统进行初始化，接着对目标进行扫描、对准、抓取，并在抓取完成后控制各轴回归到记录点；该设置根据当前的模式以及相应的条件和动作来进行扫描、对准、抓取、回归模式之间的切换，通过这种模式的转换可以快速准确的采摘下目标，使得通用性强，可扩展性高。

实施例2

本发明实施例2提供的基于有限状态自动机架构的采摘机器人控制器与实施例1基本相同，不同的是，所述初始化模块1、所述扫描模式模块2、对准模式模块3、所述抓取模式模块4以及回归模式模块5根据状态转移公式进行模式的切换：

状态转移公式为：

其中，e表示条件，a表示动作。

状态转移表

本发明中通过状态转移公式清楚地表示出了基于当前模式以及条件和动作进行模式之间的切换，通过这种模式之间的切换可以达到快速准确采摘目标的目的，状态转移公式可以配合状态转移表来使用，这样可以更加清楚的表示出各种模式之间是如何进行切换的，且如何通过这种切换来采摘下目标的。

实施例3

本发明实施例3提供的基于有限状态自动机架构的采摘机器人控制器与实施例1基本相同，不同的是，如图2所示，所述初始化模块1包括总初始化模块10和找零模式模块20，所述总初始化模块10包括：

通讯初始化模块101：用于与上位机通讯，且进行上位机的通讯模块初始化，并判断系统初始化是否完成；

返回模块102：用于基于系统初始化未完成时，返回通讯初始化模块101；

管理模块103：用于基于系统初始化完成时，进行全局变量的初始化，在全局变量初始化完成后，向找零模式模块20发送指令。

如图3所示，本实施例中所述找零模式模块20包括：

第一判断模块201：用于实时检测是否有x轴、y轴或z轴的零点标志产生，如果均有，向第一跳转模块207发送指令，如果有一个轴没有，向开关状态判断模块202发送指令；

开关状态判断模块202：用于判断是否接收到x轴、y轴或z轴对应的开关信号，如果接收到开关信号，向第一设置模块203发送指令，如果未接收到开关信号，向反零运动标志判断模块204发送指令；

第一设置模块203：用于生成x轴、y轴或z轴的反零运动标志，并向x轴、y轴或z轴发送反零方向运动的命令，同时设置x轴、y轴或z轴的反零运动单位脉冲，再向开关状态判断模块202发送指令；

反零运动标志判断模块204：用于判断x轴、y轴或z轴反零运动标志是否产生，如果反零运动标志产生，向第三设置模块206发送指令，如果反零运动标志未产生，向第四设置模块31发送指令；

第四设置模块205：用于向x轴、y轴或z轴发送向零方向运动的命令，设置x轴、y轴或z轴的向零运动单位脉冲，并向第一判断模块201发送指令；

第三设置模块206：用于向x轴、y轴或z轴发送停止运动的指令；获取x轴、y轴或z轴电机的当前脉冲点，当前脉冲点为x轴、y轴或z轴的零点值，x轴、y轴或z轴零点标志产生，并向第一判断模块201发送指令；

第一跳转模块207：用于向扫描模式模块2发送指令。

如图4所示，本实施例中扫描模式模块2包括：

第一获取模块21：用于获取z轴的零点值以及z轴的运动信息、目标存在状态信息，所述运动信息包括z轴运动的最低点和最高点，所述目标存在状态信息包括目标存在标志、目标位置坐标；

第二判断模块22：用于判断是否存在目标存在标志，如果存在，向第二跳转模块23发送指令，如果不存在，向第一管理模块24发送指令；

第二跳转模块23：用于记录当前时刻z轴位置，并产生目标存在标志，向对准模式模块3发送指令；

第一管理模块24：用于获取z轴相对位置，设置其为扫描格度值，并设置z轴在单位周期内走过的距离；

第三判断模块25：用于根据扫描格度值与z轴的零点值判断z轴运动方向，当为向零运动时，向第一计算模块26发送指令，当为反零运动时，向第二计算模块29发送指令；

第一计算模块26：用于计算扫描格度值与z轴在单位周期内走过的距离的差值；

第四判断模块27：用于根据第一计算模块26所得的差值判断是否到达z轴运动的最低点，如果到达z轴运动的最低点，向第二管理模块28发送指令，如果未到达z轴运动的最低点，向第一获取模块21发送指令；

第二管理模块28：用于设置z轴的运行方向为反零方向，并向第一获取模块21发送指令；

第二计算模块29：用于计算扫描格度值与z轴在单位周期内走过的距离的和值；

第五判断模块30：用于根据第二计算模块29所得的和值判断是否到达z轴运动的最高点，如果到达z轴运动的最高点，向第三管理模块301发送指令，如果未到达z轴运动的最高点，向第一获取模块21发送指令；

第三管理模块301：用于设置z轴的运行方向为向零方向，并向第一获取模块21发送指令。

如图5所示，本实施例中对准模式模块3包括：

第五设置模块31：用于设置x轴误差阈值和z轴误差阈值；

第二获取模块32：用于获取目标存在状态信息以及x轴、y轴和z轴的最大行程值，所述目标存在状态信息包括目标存在标志、目标位置坐标，计算目标位置坐标和第二跳转模块23确定的位置坐标之间的x差值和z差值；

第六判断模块33：用于判断目标存在标志是否产生，如果目标存在标志未产生，向回归模式模块5发送指令，如果目标存在标志产生，向第七判断模块34发送指令；

第七判断模块34：用于判断x轴、y轴和z轴是否均到达对应轴的最大行程值，如果是，向回归模式模块5发送指令，如果不是，向第八判断模块35发送指令；

第八判断模块35：用于判断x差值、z差值是否小于对应的x轴误差阈值、z轴误差阈值，如果有一差值大于误差阈值，向第四管理模块36发送指令，如果x差值、z差值均小于对应的x轴误差阈值、z轴误差阈值，向第五管理模块37发送指令；

第四管理模块36：用于根据x差值或z差值计算对应轴偏差脉冲数，向x轴或z轴发送以x轴偏差脉冲数或z轴偏差脉冲数运行的指令，并向第二获取模块32发送指令；

第五管理模块37：用于向x轴或z轴发送停止运动的指令，同时设置y轴偏差脉冲数，并向y轴发送以y轴偏差脉冲数运行的指令；

第六管理模块38：用于判断是否接收到触及目标的信号，如果接收到触及目标的信号，向抓取模式模块4发送指令，如果未接收到触及目标的信号，向第二获取模块32发送指令。

如图6所示，本实施例中抓取模式模块4包括：

第一控制模块41：用于向剪刀的电磁阀发送导通指令；

计时模块42：用于在电磁阀导通时开始计时；

第二控制模块43：用于在计时模块42记录到时间到达预定时间时，向剪刀的电磁阀发送关闭指令，并跳转到回归模式模块5。

如图7所示，本实施例中回归模式模块5包括：

第六设置模块51：用于设置x轴的行程中点为记录点，y轴零点值为记录点，第二跳转模块23记录的z轴位置为z轴的记录点，并设置x轴、y轴、z轴的距离误差阈值；

第三获取模块52：用于获取目标存在标志，并在目标存在标志不存在时，获取x轴、y轴、z轴的位置缓冲值及记录点，位置缓冲值为当前位置与记录点之间的行程值，并计算回归后的位置与记录点之间的x距离误差值、y距离误差值、z距离误差值；

回归模块53：用于基于x轴、y轴、z轴的位置缓冲值，分别向x轴、y轴、z轴发送回归记录点的指令；

第十判断模块54：用于判断x距离误差值、y距离误差值、z距离误差值是否小于对应的x轴的距离误差阈值、y轴的距离误差阈值、z轴的距离误差阈值，如果是，向扫描模式模块2发送指令，如果有一距离误差值不小于距离误差阈值，向第七管理模块55发送指令；

第七管理模块55：用于设置x轴、y轴或z轴的回归速度脉冲数，向x轴、y轴或z轴发送以回归速度脉冲数运行的指令，并向第三获取模块52发送指令。

本发明中对初始化模式模块、扫描模式模块、对准模式模块、抓取模式模块、回归模式模块进行了具体的限定，且通过各模块的具体内容可以清楚的了解到该控制器是如何工作的，并且了解到如何通过模式之间的切换来达到快速准确的采摘目标的目的。

本发明中初始化模块包括总初始化模块和找零模式模块，总初始化模块用来在控制器开启时，对系统进行初始化，初始化完成后，各轴开始找零，只有在各轴均找到零点后才能进入扫描模式模块，而x轴、y轴、z轴的找零可以同时进行，也可以在一个轴找零完成后停止运动再进行下一个轴的找零；扫描模式模块用来对采摘的目标进行扫描，并在找到目标后进入对准模式，对准模式用来控制各轴对准采摘目标，并在对准目标后，抓取模式将目标摘下，接着回归模式控制各轴回归到记录点，然后继续进行目标的扫描，这种各模式之间的切换以及循环可以实现快速准确采摘下目标的目的。

本发明中的采摘机器人在y轴上设置有剪刀，剪刀上连接有控制剪刀工作的电磁阀，在对准模式控制各轴对准目标后，抓取模式控制剪刀剪下目标。

实施例4

本发明实施例4提供的基于有限状态自动机架构的架构方法，如图8所示，所述架构方法包括如下步骤：

s1：通过初始化模块1对系统进行初始化，并判断初始化是否完成；

s2：通过扫描模式模块2在系统初始化完成后，对采摘目标进行扫描，并判断是否有目标存在；

s3：通过对准模式模块3基于有目标存在时，控制采摘点接近采摘目标，并判断是否触及目标；

s4：通过抓取模式模块4基于采摘点触及采摘目标时，控制采摘点剪下目标；

s5：通过回归模式模块5进入回归模式，控制各轴回归记录点。

本发明提供一种新的基于有限状态自动机架构的架构方法，该架构方法基于有限状态自动机实现对目标的快速采摘，工作时，首先对系统进行初始化，接着对目标进行扫描、对准、抓取，并在抓取完成后控制各轴回归到记录点；该设置根据当前的模式以及相应的条件和动作来进行扫描、对准、抓取、回归模式之间的切换，通过这种模式的转换可以快速准确的采摘下目标，使得通用性强，可扩展性高。

以上所述实施例仅仅是本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的权利要求书确定的保护范围内。