一种新型的果园作业机器人装置及方法与流程

本发明属于机器人导航，具体涉及一种新型的果园作业机器人装置及方法。

背景技术：

近几年，果园作业机器人虽然已有应用，但果园机器人的作业对象和工作环境具有较高的不确定性，给其设计带来了较大的挑战，尤其是给实际应用带来了较大的困难。由于果树枝叶生长方向各向的不确定性，使机器人较难实现自主导航；在果园喷洒农药时，较难实现均匀喷药；在果实成熟需要采摘时，也有一系列的问题需要解决，而在山地果园作业时，也还需要提高机器人的自适应能力。

现有的轮式机器人，其运动的稳定性和作业环境及路况有关，在复杂的果园地形上难以施展其优势；使用传感器来测量导航轨迹和机器人实际行进的偏差，根据测量值对位姿进行测算的机器人，其导航只能在很小的角度内进行。当机器人在不平坦的果园地面工作时，机器人产生的上下摆动会对机器人的稳定性造成影响。

申请公布号为CN105974928A的发明专利申请公开了“一种机器人导航路径规划方法”，该方法包括：设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置的所有目标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引；输入机器人当前路标和目标路标；确定当前路标与目标路标的层级；搜索当前路标与目的路标的共同关系路标；确定所述路径为当前路标到共同关系路标与共同关系路标到目标路标的路径，路径规划完成。该技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级，将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中算法简单、效率高。规划的路径唯一，很适合在嵌入式设备上运行。但该方法轨迹规划不够精确，无法适用于果园复杂的地形。

申请公布号为CN106017477A的发明专利申请公开了“一种果园机器人的视觉导航系统”，该系统包括导航基准线识别模块，二维码定位模块，超声波避障模块和单片机控制模块四部分。该发明将基于自然环境进行导航基准线识别与基于二维码标识进行定位相结合进行视觉导航，采用超声波传感器进行安全避障，最后由单片机对果园机器人进行行走控制。该发明对于降低导航成本、解决单纯基于自然环境进行视觉导航在果园地头转弯适应性差的问题，以及提高导航的可靠性与实用性具有重要意义。但该系统没有考虑果树枝叶生长的不确定性，存在视觉盲区，无法实现全方位定位。

申请公布号为CN105867386A的发明专利申请公开了“一种机器人导航系统”，包括传感器模块、判断模块及策略控制模块。传感器模块用于发射距离信息，接收返回的距离信息，并根据返回的距离信息进行计算，并进行地图构建及输出机器人的位置信息。判断模块用于根据机器人的位置信息来判断机器人是否是处于安全区域。策略控制模块用于在获知机器人处于安全区域时，控制机器人采用第一运行策略进行移动，或在获知机器人处于非安全区域时，控制机器人采用第二运行策略进行移动。本发明还公开了一种机器人导航方法。上述机器人导航系统及方法，兼顾机器人的运行效率和应对复杂环境的可靠性，提高机器人的移动效率及安全性。但该系统步骤繁琐，且无法高效适用于果园的复杂地形。

技术实现要素：

本发明的目的在于克服现有技术的不足，提出一种新型的果园作业机器人装置及方法。通过采用履带式机器人，增加姿态角传感器和位置角传感器，增加调节架，提高对土壤的保护性，使机器人越障平稳，具有自动复位的功能，使机器人在较大的角度内进行导航，提高果园作业的控制精度和精细化程度，使导向轮和导向管稳定地结合，降低摆动对机器人的影响，提高机器人在不平稳地面上作业的稳定性和对果园复杂地形的适应能力，满足果园作业机器人的设计需求。

本发明解决上述技术问题的技术方案是：

一种新型的果园作业机器人装置及方法，其特征在于，所述装置包括：机器人导向机构模块、电位传感器模块、数据采集卡模块、计算机模块、电机系统模块及机器人本体模块；其中，所述机器人导向机构模块与电位传感器模块相连；所述电位传感器模块与数据采集卡模块相连；所述数据采集卡模块与计算机模块相连；所述计算机模块与电机系统模块相连；所述电机系统模块与机器人本体模块相连；所述机器人本体模块与电位传感器模块相连。

进一步地，所述机器人本体模块用于具体实施动作。

进一步地，所述电位传感器模块用于测量导航轨迹和机器人实际行进的偏差。

进一步地，所述机器人导向机构模块用于进行轨迹规划和导航。

进一步地，所述数据采集卡模块用于把采集到的模拟量转换为数字量。

进一步地，所述电机系统模块用于电机驱动和控制电机动作。

所述方法包括：S1，机器人导向机构通过基于自适应的遗传算法和样条曲线进行轨迹规划和导航；S2，电位传感器模块通过测量导航轨迹和机器人实际行进的偏差，并根据测量值对位姿进行测算，得到导向机构的位姿偏差；S3，数据采集卡模块把采集得到的模拟量转换为数字量；S4，计算机模块接收并处理数字量；S5，根据数字量，电机系统模块实现机器人的跟踪。

本发明与现有技术相比具有以下的有益效果：

本发明通过采用履带式机器人，增加姿态角传感器和位置角传感器，增加调节架，提高对土壤的保护性，使机器人越障平稳，具有自动复位的功能，使机器人在较大的角度内进行导航，提高果园作业的控制精度和精细化程度，使导向轮和导向管稳定地结合，降低摆动对机器人的影响，提高机器人在不平稳地面上作业的稳定性和对果园复杂地形的适应能力，满足果园作业机器人的设计需求。

附图说明

图1是新型的果园作业机器人装置的控制系统说明图。

图2是机器人导航编码流程示意图。

图3是电机控制结构图。

具体实施方式

下面结合附图及具体实施例对本发明进行更加详细与完整的说明。可以理解的是，此处所描述的具体实施例仅用于解释本发明，而非对本发明的限定。

如图1所示，本发明的一种新型的果园作业机器人装置及方法，其中，所述装置包括：机器人导向机构模块、电位传感器模块、数据采集卡模块、计算机模块、电机系统模块及机器人本体模块；其中，所述机器人导向机构模块与电位传感器模块相连；所述电位传感器模块与数据采集卡模块相连；所述数据采集卡模块与计算机模块相连；所述计算机模块与电机系统模块相连；所述电机系统模块与机器人本体模块相连；所述机器人本体模块与电位传感器模块相连。

所述机器人导向机构模块用于轨迹规划和导航；所述机器人本体模块用于具体实施动作；所述电位传感器模块用于测量导航轨迹和机器人实际行进的偏差；数据采集卡模块用于把采集到的模拟量转换为数字量；计算机模块接收并处理数字量；电机系统模块用于电机驱动和控制电机动作。

所述方法包括：

S1，机器人导向机构模块通过基于自适应的遗传算法和样条曲线进行轨迹规划和导航。

果园作业机器人的自主导航主要依赖于运动路径的规划。机器人运动规划一般分为两种方法:一种方法是轨迹规划，另一种方法是路径规划。路径规划一般是机器人按照某一性能指标搜索得到最优的规划路径；而轨迹规划是指机器人的各个节点在空间坐标上形成时间序列，包括位移、速度和加速度等。为了使机器人轨迹规划更加精确，引入了三次多项式插值的方法

具体过程包括：考虑果园作业机器人在一定的时间内从初始位姿移动到目标位姿问题，首先利用逆运动学原理，求出起始位置和终止位置的位移；然后求解机器人通过初始位置和终止位置的光滑函数，将机器人在初始位置和终止位置的速度设置为0，将时间设定为自变量，利用三次多项式进行拟合，保证机器人移动速度的连续性。其中，三次多项式的表达式为：

θ(t)＝a₁t³+a₂t²+a₃t+a₄ (1)

其中，θ为广义坐标；t为时间；a为三次多项式的系数。初始和终止的约束条件为

其中，T为一个周期内机器人控制的总时间。因此得到三次多项式的计算系数为

果园作业机器人路径规划最短的样条曲线优化目标函数可以写成

其中，T为一个周期内机器人控制的总时间；x_i为第i段样条曲线的运动时间；n为型值点的点数；ξ为约束系数；ξ＞0；τ为机器人的角速度、角加速度、角加加速度和移动力矩；Θ，t_max为关节角速度约束、角加速度约束、角加加速度约束和关节力矩约束；ture为二值表达式，当超过约束时，记作1，降低适应度值，在约束以内时为0。适应度函数取目标函数的倒数，记作

假设机器人第i部分的关节角速度为

则有

其中，q'和t'的数值由控制位置的横坐标和纵坐标决定，对x'值进行优化会直接影响t'值的大小，最大角速度发生的位置在t_i或者(t_i,，t_i+1)区间，则

其中，为最大角速度的绝对值；|θ|(i＝1,2,…,n-1)为在t_i时刻角速度的绝对值；为在区间(t_i,t_i+1)绝对值的最大值，自适应遗传算法的具体步骤如下：

1)确定群体的规模、变量维数、交叉概率、变异概率、集散因子和迭代次数，确定型值点；

2)求出型值点，计算机器人的移动控制点；

3)利用自适应编码的方法建立初始种群；

4)计算机器人每条染色体对应的时间控制点；

5)利用样条曲线计算染色体对应的速度、角速度、角加速度、角加加速度和关节力矩对应的数值，并判断数值是否超过约束的范围；

6)计算机器人移动本体的适应度值，按照适应度的大小进行排序；

7)计算种群的平均适应度值，并将此作为阈值，选取最大的平均适应度个体，加入子代个体；

8)将机器人移动速度较低的个体和优秀个体进行交叉，替代适应度较低的个体，使个体在约束范围内，并自动加入到子个体中；

9)通过自适应遗传算法确定交叉和变异的概率；

10)进行交叉和变异，产生下一代种群；

11)计算个体适应度的最大值，判别适应度值的范围，选取适应值较大的值，来替代种群中较差的染色体；

12)当满足终止条件时，停止计算；否则，进入步骤4)，直到计算停止。

S2，电位传感器模块增加了姿态角传感器和位置角传感器，可使机器人在较大的角度内进行导航。

S3，数据采集卡模块用于把采集到的模拟量转换为数字量。

S4，计算机模块接收并处理数字量；计算机对机器人导航系统控制采用VC++编程的方式，通过编码实现系统的自动化控制。

如图3所示，在程序的编制过程中，主要考虑对系统各种构件的保护，因此当导航位置角和姿态角大于一定数值时，机器人需要具有自动停止的功能，控制器输出的电机PWM控制值也需要在一定的范围之内。根据传感器滤波和信号采集及信号传输与控制所需要的时间，机器人单个导航周期设定为100ms。

S5，电机系统模块中，机器人两侧履带分别用一个直流4V电机驱动，上位机利用串口和Modbus控制卡连接，控制卡可以同时控制左侧和右侧两路电机驱动器，来控制伺服电机的动作。上位机和Modbus控制卡采用Modbus工业通信协议。

以上所述仅为本发明的优选实施例，并不用于限制本发明，对于本领域技术人员而言，本发明可以有各种改动和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。