[发明专利]一种基于果园地图的异构多机器人定位和控制方法

说明书

本发明公开了一种基于果园地图的异构多机器人定位和控制方法，具体包括：由机器人先构建出完整的果园地图，再通过GNSS坐标系转换，将果园地图中所有的果树中心点，换算成GNSS绝对坐标值[treesubgt;i/subgt;_gnss_x,treesubgt;i/subgt;_gnss_y]；领航机器人可以根据雷达扫描的角度和相对距离估计出该范围内最近的果树坐标[treesubgt;n/subgt;_gnss_x,treesubgt;n/subgt;_gnss_y]，将其与果园地图中的各果树真实坐标[treesubgt;i/subgt;_gnss_x,treesubgt;i/subgt;_gnss_y]对比，当距离差dsubgt;min/subgt;最小时，判定为最靠近机器人的果树GNSS真实坐标，最后通过果树的GNSS真实坐标来预估计多机器人在果园中的准确位置和航向，完成果园行间作业和地头转弯作业。本发明在领航‑跟随型异构机器人协同作业基础上，利用果园地图和各机器人不同导航方式的优势，准确定位多机器人在果园中的作业位置，实现多机器人在果园中协同作业。

技术领域

本发明属于果园机械领域，具体涉及一种基于果园地图的异构多机器人自主导航控制方法。

背景技术

随着越来越多的果园采用集约化种植经营的模式，单机作业已经不能满足果园季节性生产的需求，采用多机作业已成为一种必然趋势。但现有果园机器人由于生产厂家不同，机器人自身所配备的传感器、控制器的精度多不相同，机器人的自主导航方式也不同，将这些异构机器人构建成一个多机器人群较为困难。而购置新的传感器或机器人，又增加了生产成本，不利于维护。

目前异构多机器人协同定位和控制的研究多在无GNSS室内环境中，通过移动机器人上的相对定位传感器互相观测对方的位置，再利用粒子滤波或扩展卡尔曼滤波算法融合机器人自身传感器信息，预估计机器人下一时刻的位置和方向，最后实现异构多机器人按照固定队形协同作业。而在果园环境中，由于果树的遮挡，机器人无法通过相对定位传感器直接观测到其他机器人的位置，需要依靠GNSS绝对信息来完成协同作业，而果园机器人配备GNSS传感器精度不一，在果树行间作业时难以获取准确的定位，更无法完成协同作业，存在定位精度和无法协同控制的问题。

发明内容

为了克服在果园环境中异构多机器人GNSS传感器精度不一，难以实现机器人定位和控制的问题。该机器人网络中有3个以上的异构机器人，且每个机器人配备的GNSS传感器精度不一，其中各机器人在果园中自主导航的方式不同，领航机器人依赖雷达导航，跟随机器人依赖GNSS导航，但构建成多机器人时无法实现精确运动。本发明提出一种基于果园地图的异构多机器人定位和控制方法，本发明采用的技术方案是：

一种基于果园地图的异构多机器人定位和控制方法，其特征在于，包括如下步骤：

领航机器人由人遥控在果树行中行走，先构建一幅完整的果园地图，再通过GNSS坐标系转换，将果园地图中所有的果树中心点，换算成GNSS绝对坐标值[tree_i_gnss_x,tree_i_gnss_y]；领航机器人可以根据雷达扫描的角度和相对距离估计出该范围内最近的果树坐标[tree_n_gnss_x,tree_n_gnss_y]，通过式(1)与果园地图中的各果树坐标[tree_i_gnss_x,tree_i_gnss_y]对比，当d_min接近0时，得到果树的GNSS真实坐标；

进一步地，距离机器人最近果树真实坐标[tree_i_gnss_x,tree_i_gnss_y]的判定计算公式为：