本发明涉及定位导航技术，具体涉及一种果园机器人及其多超声视频点组合导航方法。
背景技术：
我国是全球最大的水果生产国，但果园工作仍以人力劳作为主，劳动强度大，生产效率低。近年来随着工业化进程的加快以及人口老龄化的加剧，农村劳动力急剧减少，以自主导航为关键技术的农业机器人越来越得到科研者的重视。在果园中，由于机器人频繁在树冠下运动，以往的gps定位导航存在着卫星信号被树枝叶遮挡、多路径效应和射频干扰等缺点；电磁定位导航用于电缆铺设和维护的成本较高，且不能满足机器人高速行走的要求；超声波定位导航感知的环境信息少，且可靠性不足；激光扫描定位导航在果树缺失时无法单独完成定位工作；视觉传感器虽可以采集大量的环境数据，但易受果园复杂环境的影响，单独工作很难满足实际要求，只能作为辅助的定位导航方式。单一的传感器很难满足果园机器人定位导航系统的精度和可靠性要求，因此，需要设计一种能够将多种传感器联合起来进行构建的定位导航系统，通过信息融合来获得更加完善的环境信息，实现快速定位导航。
技术实现要素：
本发明的目的在于针对上述现有技术中的问题，提供一种果园机器人及其多超声视频点组合导航方法，成本较低、适用性强，能够满足果园机器人行间高精度作业的要求。
为了实现上述目的，本发明果园机器人采用的技术方案为：
包括行进机构以及设置在行进机构上方的定位导航单元安装平台，定位导航单元安装平台的四周安装有定位导航单元，所述的定位导航单元具有能够通过控制模块调节拍摄角度的摄像头以及能够与摄像头随动的超声波传感器；在定位导航单元安装平台的上方通过支撑立柱固定有采摘平台，采摘平台上设置有采摘机械抓以及果实收纳盒。
定位导航单元安装平台采用与果园植株间距相配合的矩形平台，定位导航单元分别固定在其四角。定位导航单元包括驱动电机以及沿竖直方向通过减速器与驱动电机相连的超声波传感器，超声波传感器上方固定有摄像头安装支架，摄像头与摄像头安装支架铰接并能够在俯仰方向调节拍摄角度；所述的驱动电机下方安装有用于记录旋转角度的旋转编码器。
本发明果园机器人的多超声视频点组合导航方法，包括以下步骤：
第一步、对定位导航单元的位置进行初始化，使果园机器人沿果园移动，通过定位导航单元的摄像头获取环境信息，并根据获取到的环境信息提取果树特征；
第二步、设定树干的截面直径阈值，取距离果园机器人最近的树干信息并确定出树干中心线，将树干中心线所在像素坐标传送给控制模块，控制模块调节摄像头的拍摄角度，使摄像头对准树干中心线，记录下摄像头相对于初始位置旋转的角度，并通过超声波传感器检测出摄像头对准树干中心线时二者之间的距离；
第三步、通过果园机器人同侧的定位导航单元测得的数据求得行驶道路边缘直线在机器人坐标系中的直线方程，使用行驶道路两侧边缘直线方程求得行驶道路中心线方程；
第四步、根据环境信息设定道路中心线阈值，重复第一、二、三步，得到多个行驶道路中心线方程，舍去超出道路中心线阈值的数据，求取行驶道路中心线方程的斜率平均值及行驶直线与机器人坐标系x轴交点与机器人坐标系原点距离的平均值，构建道路中心线方程；
第五步、根据行驶道路中心线方程的斜率平均值及行驶直线与机器人坐标系x轴交点与机器人坐标系原点距离的平均值得到道路中心线与果园机器人中心线之间的偏角以及距离，将数据传送给控制模块来消除偏差，完成对果园机器人的定位导航。
所述的第一步在进行初始化时假定四组摄像头的水平工作范围为
所述的第二步当四组摄像头全部对准树干中心线时，记录此时摄像头相对于初始位置旋转的角度
左1：
左2：
右1：(a+r1cosθ1，b+r1sinθ1)；
右2：(a+r2cosθ2，-b-r2sinθ2)；
通过以上坐标求解出行驶道路边缘直线在机器人坐标系中的直线方程，并使用行驶道路两侧边缘直线方程求得行驶道路中心线方程，表示为y＝kx+b。
所述的第四步在果园机器人前进的时间△t内，重复第一、二、三步，得到n个行驶道路中心线方程，其斜率为k1～kn，行驶道路中心线与机器人坐标系x轴交点到机器人坐标系原点的距离为x1～xn，根据环境信息设定道路中心线阈值，在n条行驶道路中心线中舍去超出道路中心线阈值的数据，求取行驶道路中心线方程的斜率平均值ka及行驶直线与机器人坐标系x轴交点与机器人坐标系原点距离的平均值xa，构建道路中心线方程y＝ka(x-xa)；
所述的第五步根据行驶道路中心线与机器人坐标系y轴间的偏角△α以及机器人偏离行驶道路中心线的距离△x，通过控制模块消除偏差，实现果园机器人沿果园行列行走。
与现有技术相比，本发明具有如下的有益效果：首先通过摄像头检测果树的位置，然后通过随动的超声波传感器测量果树与摄像头之间的距离，分布在定位导航单元安装平台四周的定位导航单元能够快速获取较为完整的果园环境信息，本发明通过将果园机器人视觉导航与超声波导航相结合，减弱了环境噪声对视觉传感器的影响，同时又弥补了超声波传感器感知环境信息少的缺点，能够不依赖于gps定位，也不必对果园环境进行人为改造，该机器人的制造成本低，方案适用性强，能够满足果园机器人行间高精度作业的要求。
附图说明
图1本发明定位导航单元的结构示意图；
图2本发明果园机器人的装配结构示意图；
图3本发明定位导航单元位姿初始化及工作范围示意图；
图4本发明定位导航方法的流程图；
图5本发明机器人与果树的相对关系场景一图；
图6本发明机器人与果树的相对关系场景二图；
图7本发明机器人与果树的相对关系场景三图；
图8本发明机器人与果树的相对关系场景四图；
图9本发明机器人与果树的相对关系场景五图；
图中：1-摄像头；2-超声波传感器；3-减速器；4-驱动电机；5-旋转编码器；6-行进机构；7-定位导航单元安装平台；8-定位导航单元；9-采摘平台；10-采摘机械抓；11-果实收纳盒。
具体实施方式
下面结合附图对本发明做进一步的详细说明。
参见图1，本发明的果园机器人包括四个定位导航单元8，定位导航单元8由摄像头1、超声波传感器2、减速器3、驱动电机4、旋转编码器5组成，摄像头1与超声波传感器2随动，在驱动电机4的驱动下水平转动。四个定位导航单元8在机器人上的安装方式如图2所示，分别安装在定位导航单元安装平台7的四角。
本实施例中果园植株间距为δd，行间距为w，机器人宽2a，长2b，摄像头视角为60°，保证机器人移动的过程中，摄像头的视野总能覆盖一颗果树。
本发明果园机器人的多超声视频点组合导航方法，包括以下步骤：
第一步，如图3所示，对定位单元的位姿进行初始化，初始位置摄像头1的朝向与行进机构6的履带缠绕方向垂直，摄像头1的水平工作范围为
第二步，设定所求树干截面直径阈值，以得到距离机器人1最近的树干信息并确定树干中心线，将树干中心线所在像素坐标传送给单片机控制模块，单片机控制模块传送指令给电机驱动模块，驱动电机4随即驱动摄像头1对准树干中心线。在此过程中，四组摄像头1分别在工作空间内往复搜索树干并对准树干中心线，先搜索到树干的摄像头1对树干中心线进行跟踪，等待其他摄像头1完成搜索工作，当四组摄像头1全部对准树干中心线时，旋转编码器5记录此时摄像头相对于初始位置旋转的角度分别为
第三步，当摄像头1对准树干时，超声波传感器2测得此时摄像头1与树干中心线之间的距离分别为l1、l2、r1、r2。
第四步，通过上述数据得到此时摄像头锁定的四棵果树中心线在机器人坐标系中的坐标：
左1：
左2：
右1：(a+r1cosθ1，b+r1sinθ1)；
右2：(a+r2cosθ2，-b-r2sinθ2)；
用该坐标求解出此时道路边缘直线在机器人坐标系中的方程，并利用两条道路边缘直线求出道路中心线的方程：y＝kx+b。
第五步，在机器人前进的时间δt内，重复第一、二、三、四步，获得n组道路中心线方程，其斜率为k1～kn，中心线与机器人坐标系xr轴交点到机器人坐标系原点距离x1～xn。由于摄像头1的视角足够大，视野中总能覆盖到一颗果树，能保证算法的快速执行。若摄像头1转到极限位置而没搜索到果树，则继续在工作空间内往复搜索直到发现树干，若发现果树缺失，即摄像头1遍历工作空间而没有发现果树，则摄像头1继续往复搜索，控制单元不发送任何控制指令，保持机器人当前行走路径不变，直到发现新的果树。
第六步，根据果园环境设定道路中心线阈值，在采集的n组中心线中舍去超出阈值的采样值，用剩余的部分求平均值得到ka、xa，方程为y＝ka(x-xa)。
第七步，根据道路中心线方程得到此时该中心线与机器人坐标系yr轴之间的偏角δα，以及机器人偏离中心线的距离△x，将数据用can总线传送给单片机控制单元消除偏差，即可保证机器人沿果园路径行驶。
摄像头工作范围的设定与果园具体环境有关，在本实施例中，机器人与果树的相对关系有5种情况，在如图6所示的场景二中，机器人前侧摄像头到达工作极限位置0°，在如图7所示的场景三中，由于距离前侧摄像头最近的树超出摄像头工作视角，所以摄像头往复搜索下一颗果树。在如图8所示的场景四中，机器人后侧摄像头到达极限位置
实际使用时留有5°到10°的余量。机器人前侧摄像头的极限工作位置
实际使用时留有5°到10°的余量。
本实施例先用摄像头1检测果树位置，然后随动的超声波传感器2测量果树与摄像头1之间的距离，四个定位导航单元8可以得到多点的角度、距离组合信息，能够快速的获取较为完整的果园相对环境信息，根据上述说明内容，以本发明技术方案为基础，从事相关工作的人员完全可以根据所要求的果园环境对本实施例进行改动，本发明的技术范围并不局限于以上的内容，凡以本发明的技术方案或机械结构为基础的任何方案均在本发明的保护范围。