适用于果园农机的多传感器融合低成本高精度定位方法

本发明属于果园导航定位，具体涉及适用于果园农机的多传感器融合低成本高精度定位方法。

背景技术：

1、我国果园机械化仍处于起步阶段，综合机械化率仅为25％，果树生产仍存在劳动效率低、生产成本高等问题，与“高产、优质、高效、生态、安全”的现代农业发展需求还存在一定差距。随着果树种植规模的不断扩大，果园农机自动化也进一步发展，导航技术在农机中的应用也越来越广泛。

2、gps定位技术成熟且广泛使用，但其定位精度较低，且在果树林立的果园中，树木的遮挡和多变的气候常常会导致gps信号丢失，无法满足果园农机导航的需要。传统的方法一般结合高精度组合惯导系统提高gps定位精度，但这种方法存在成本较高、无法满足长时间工作的高精度导航系统的指标要求等问题，无法广泛使用在果园农机中。

3、高精度地图定位可以对农机的位置进行有效的信息补充，在极端的天气或者复杂的果园环境下，仅仅依靠硬件设备的采集很难正确分辨路面信息，高精度地图定位便在此起到重要作用。但高精度地图更新较慢，且仅靠高精度地图位置信息范围大，计算复杂度高。

4、视觉定位成本低廉、精度较高，具有短距离导航精度高的特性，但是由于果林里环境复杂，存在树荫，光线环境多变，有时会造成视觉定位的失效。由于摄像头的局部感知特性，在复杂多变的作业环境下常常使用视觉定位实现局部的环境感知。

5、目前，多传感器融合定位的方法已经得到了广泛应用，如何在果园环境下，融合多种传感器的优势，在有限的成本内提高农机定位系统的准确性、实时性和稳定性，是果园农机定位导航面临的重要问题。

技术实现思路

1、为解决上述问题，本发明提供了适用于果园农机的多传感器融合低成本高精度定位方法，利用gps、视觉与高精度地图的位置信息获得融合定位数据。该方法在离线阶段完成高精度地图数据采集和处理，从路面图像特征、果树的位置和高度等数据对果园路面进行表征，通过视觉融合惯导技术得到生成高精度地图，使得地图中的点位包含图像信息、高精度惯导数据以及路面结构特征，使之适用于果园环境下农机的高精度定位；在线阶段通过采集农机上的gps坐标获取农机初步位置，并转换为空间平面坐标系下的坐标。同时确定高精度地图的搜索范围，选取地图中采样点处定位数据与农机当前位置点p最近的点位，匹配成功后即可获取高精度地图定位的位置数据；再采用单目摄像机采集视频信息，通过基于双平行线特征的位姿估计解析算法获取视觉定位数据；最后采用d-s-cnn算法融合gps、高精度地图、视觉定位获得的数据，获取农机在果园中更加精确的位置坐标。

2、为了达到上述目的，本发明提供如下技术方案：

3、适用于果园农机的多传感器融合低成本高精度定位方法，包括如下步骤：

4、步骤1，gps定位：

5、记录农机在果园作业时某一时刻位置为点p，通过农机上的gps采集设备测定到p点的经纬度坐标；采用卡尔曼滤波方法对采集的经度数据和纬度数据分别进行处理，再将处理后的数据转换为空间平面坐标系下面的坐标pg；

6、步骤2，高精度地图定位：

7、利用步骤1中的gps做出初始位置判断，确定高精度地图的局部搜索范围，将实时定位数据与特征点进行距离度量，计算农机当前位置点p与每个特征点之间的欧氏距离，根据距离度量结果，选择距离最近的特征点作为匹配结果，匹配成功后即获取高精度地图定位的位置数据pm；

8、步骤3，视觉定位：

9、农机视觉定位采用果树作为直线检测，采取使用单目摄像机采集视频信息，通过基于双平行线特征的位姿估计解析算法获得位置信息pv；

10、步骤4，基于d-s-cnn的融合定位：

11、4-1基于d-s证据理论同步分析多个传感器实时可信度，包括如下子步骤：

12、4-1-1确定基本信任分配函数：

13、将步骤1、2、3中的传感器定位数据归一化处理后作为证据，令e1＝{pg},e2＝{pm},e3＝{pv}，得g1,g2,g3；其中g为2d上的基本信任分配函数，e为集合d的子集，g(a)表示对e的信任程度大小；g：2ω→[0，1]；

14、4-1-2利用证据组合规则建立信任函数矩阵kc：

15、将传感器数据进行归一化处理后作为证据，用证据组合规则进行证据组合，产生新的信任函数矩阵，记为kc；取正交和g＝g1+g2+g3，则：

16、

17、

18、其中信任函数kc的取值为矩阵中1表示传感器判决为有效，0表示判决为无效；

19、4-2利用cnn结合实时可信度进行多传感器信息融合：

20、完成cnn神经网络离线训练，将pg,pm,pv,kc作为输入，包括如下子步骤：

21、4-2-1通过卷积层进行特征提取：

22、卷积层的计算过程如下：其中为第n层第j个节点的计算结果；σ(·)为激活函数；为第n层第j个输出特征图所对应的多个输入特征图的索引集合；为偏置项，所有输入特征图共用这一个偏置项；为卷积核；

23、4-2-2通过池化层进行特征映射：

24、池化层的计算过程如下：其中down(·)为下采样函数；为乘性偏置；

25、4-2-3通过连接层进行特征分类：

26、再次对数据进行降维操作，其维度进一步趋向于输出维数；

27、4-2-4通过输出层进行位姿输出：

28、完成神经网络的传感器信息综合，得到神经网络输出的农机的位姿信息，该输出的定位结果即为农机精确定位结果(x',y')，记为p'；再将p'转换为经纬度数据(b',l')＝trans-1(x',y')，即得到在果园环境下基于d-s-cnn算法融合gps、高精度地图和视觉的农机精确定位数据。

29、进一步的，所述步骤2中高精度地图通过以下方式得到：

30、首先使用测量仪器和设备测量果树周边环境地理要素的坐标信息，同时运用gps接收机、组合惯导系统以及dgps基站记录采集点的位置和姿态信息；运用采集到的数据生成高精度地图，此时地图中的点位包含果树位置、高精度惯导数据以及路面结构特征；位置信息数据记为g＝{(bm1,lm1),(bm2,lm2),(bm3,lm3),…}，bmi,lmi分别表示第i个点位的纬度值和经度值，再将处理后的数据根据转换公式trans()转换为空间平面坐标系下面的坐标(xmi,ymi)＝trans(bmi,lmi)，其中(xmi,ymi)是高精度地图中第i个点位的空间平面坐标系的坐标。

31、进一步的，所述步骤2中，基于最近邻匹配的方法实现距离度量。

32、进一步的，所述步骤3具体包括如下过程：

33、首先对相机内参数进行标定，再利用标定后的单目相机采集农机在果园里的运动视频，基于帧间图像特征匹配的视觉导航方法，获取图像序列，以果树组成的平行四边形为特征提取主要对象，获取双平行线的平行关系以及其围成的面积，通过基于双平行线特征的位姿估计解析算法，利用提取的特征和位姿信息即得到视觉定位的平面坐标pv。

34、进一步的，所述步骤3中，利用张正友标定法对相机内参数进行标定。

35、与现有技术相比，本发明具有如下优点和有益效果：

36、(1)本发明创新性地将gps定位、高精度地图定位、视觉定位相结合，运用到果园农机的定位导航中，具有成本低、速度快、精度高、稳定性好等显著优点，可实现高精度的果园农机定位。

37、(2)本发明方法对三种定位方法取长补短：gps定位技术成熟且广泛使用，但其定位精度较低，在果园中由于树木的遮挡和多变的气候常常会导致gps信号丢失，无法满足果园农机导航的需要；视觉定位成本低廉、精度较高，具有短距离导航精度高的特性，但是由于果林里环境复杂，存在树荫，光线环境多变，有时会造成视觉定位的失效；高精度地图定位可解决因长期恶劣天气、光线遮挡等环境因素导致的路面不清和信号丢失影响定位数据的问题，但全局计算冗杂、更新慢。本发明采用d-s-cnn算法融合三种定位数据，可以用gps信号确定大概范围，用视觉定位修正定位结果，用高精度地图补充相关信息，同时也可避免传感器失效等意外情况无法获取定位信息等问题，故本发明创造性地利用了三种定位的优势，为果园农机实现高精度定位提供了一种思路。

38、(3)本发明克服了gps定位精度低、高精度地图更新慢算法复杂和视觉定位稳定性差等问题，可有效提高果园环境下农机gps定位的精度，融合定位后的误差为厘米级。

39、(4)本发明可有效降低农机成本，相比于传统的激光雷达定位导航安装高度较低，在果园中易受污而影响检测且成本较高，且其高速转动部件难以适应果园强振动环境。本方法采用gps传感器、摄像机并结合高精度地图，有效降低了农机高精度定位的成本，并且更适应起伏较大的果园地面。

40、(5)本发明实现了全局坐标系下的定位，为农机在果园环境种提供准确决策和规划。