一种GPS拒止环境下农机车体田间导航定位系统及方法

本发明属于农机田间导航定位，尤其涉及一种gps拒止环境下农机车体田间导航定位系统及方法。

背景技术：

1、实现农机车体智能化是国内外农机车体研究领域共同追求的前沿技术，也是缩小我国与发达国家农业经济发展水平的必然要求。农业机械自动导航技术是实现智能农业的核心，可以有效减轻农机车体操作人员的劳动强度，提高作业精度与作业效率。近年来，不同学者提出了很多农机车体导航定位的方法，农机车体自动驾驶技术中常用的定位方法包括gps定位、机器视觉定位和惯性导航定位等，gps定位系统在户外空旷的环境中，定位精度可达厘米级，但是由于环境的影响和障碍物的遮挡，gps的信号被削弱或者无gps信号，导致gps定位系统难以提供连续可靠的定位精度。视觉传感器容易受到光照的影响，同时在曝光较强、低亮度等情况下同时定位容易丢失；惯性导航定位技术不受信号的遮挡，但是惯性测量元件随着作业时间和距离的增加存在漂移现象，农机车体经过几轮作业后，惯性导航的定位误差不断累计增大，定位精度逐渐下降。

2、单一传感器都有一定的局限性，为提高导航定位精度和可靠性，常采用多传感器融合定位。申请号：202110124820.1，公开了一种基于uwb的农业机械田间定位系统及方法，将uwb定位标签和imu惯性测量单元安装在农机车体上，基站安装在定位田块的边缘，利用uwb的定位结果对imu进行修正补偿，提升农机车体的定位精度，但是当uwb定位标签的通讯范围不在基站的范围内时，无法进行融合定位。申请号：202011424886.4，公开了：一种基于激光雷达的导航车辆障碍物探测方法，通过在农机车体上上安装激光雷达和卫星天线，判定车辆前方可能存在的障碍物，但是在没有卫星信号的情况下，很难获得准确的定位精度，将会导致障碍物判断不准确。申请号：201710646549.1，公开了一种自主导航拖拉机夜间全景视觉相对定位系统和方法，将三组双目视觉系统呈等边三角形方式排列，安装在拖拉机的顶部，三组双目视觉系统同时对各自方向上环境中的同一目标进行检测，据此反推农机车体运动，形成不同坐标系下的位移矢量，再转换到同一坐标系下，实现相对定位，但是该方法难以构建三维导航地图，也无法对障碍物位置的初判断。尤其是在gps拒止环境下，如何为农机车体提供高精度的定位和避障是实现农机车体智能化作业的关键。

3、针对gps拒止环境下，传统的单一定位技术难以难满足农机车体长期自主循环作业的需求，而且定位精度较低，不能满足农机车体自动化作业的要求，距离农机车体自主精准作业仍有很大的差距，为此需要新的技术和方法来解决农机车体的长期高精度自主定位问题。

技术实现思路

1、本发明实施例的目的在于提供一种gps拒止环境下农机车体田间导航定位系统及方法，旨在解决上述背景技术中提出的问题。

2、本发明实施例是这样实现的，一种gps拒止环境下农机车体田间导航定位系统，包括：

3、农机车体、激光雷达、超宽带雷达信号发生接收模块、双目相机、imu测量模块、uwb定位基站、uwb定位终端模块、移动导轨、数据处理单元、工控机、控制模块、固定杆、移动电源、高精度姿态传感器、数据显示系统、连接固定件、支架、第一固定装置、第二固定装置、固定夹、滑块、三角架、电机；

4、在农机车体的顶部安装所述第一固定装置，在农机车体的正前方安装所述第二固定装置，所述第一固定装置和第二固定装置均包含三层，在所述第一固定装置上安装工控机，在所述第一固定装置最上层安装uwb定位终端模块，所述第一固定装置的中间层布设有imu测量模块；在所述第二固定装置上，由底部到顶部分别安装有双目相机、激光雷达和超宽带雷达信号发生接收模块；所述uwb定位终端模块通过固定夹固定在所述第一固定装置上，所述超宽带雷达信号发生接收模块通过所述固定夹固定在所述第二固定装置上，其余若干传感器通过螺栓固定在对应的固定装置上；

5、所述移动导轨通过螺栓安装固定在所述三脚架上方，所述三脚架设置在定位农田边界的边缘区域，所述固定杆通过螺栓固定在所述移动导轨的滑块上，所述高精度姿态传感器固定在所述滑块上，所述连接固定件安装在所述固定杆上，所述支架安装在所述连接固定件上，所述uwb定位基站通过所述固定夹安装在所述支架上，所述连接固定件通过螺栓固定在所述固定杆上；

6、所述imu测量模块将采集的数据传输至所述工控机中的数据处理单元，所述数据处理单元根据接收的所述imu测量模块的测量数据转换成农机车体的位置和姿态；

7、所述uwb定位终端模块实时获得农机车体作业过程中与4个所述uwb定位基站之间的距离并通过无线通讯传输至所述工控机中的数据处理单元，所述数据处理单元根据采用定位算法解算农机车体的三维位置坐标，所述定位算法具体为：鲁棒性加权最小乘法、总体最小二乘法、多维标定算法；

8、所述激光雷达实时采集农机车体作业时的点云数据并传输至所述工控机中的数据处理单元，所述数据处理单元对点云数据进行点云去噪、纠正和分割处理，获得三维点云数据的不规则三角网，利用三次样条曲线对三角网进行插值处理，然后在不规则三角网中对点云数据进行精准配准，获得农机车体作业的三维场景模型；

9、所述双目相机对农机车体前方的作业环境进行图像采集并将图像传输至所述工控机中的数据处理单元，在所述数据处理单元中对图像进行特征点检测与匹配，利用视差原理计算图像中特征点的空间坐标；

10、所述超宽带雷达信号发生接收模块将接受的信号传送至所述工控机中的所述数据处理单元，在所述数据处理单元中使用成像算法获得农机车体前方扫描区域的成像，从而获得农机车体与前方障碍物的距离，所述的成像算法具体为：压缩感知成像算法、聚束式成像算法；

11、所述高精度姿态传感器通过蓝牙数据发送模块将姿态数据发送到蓝牙数据接收模块，数据处理单元中解算uwb定位基站移动后的三维坐标；

12、所述工控机发送信号给所述激光雷达、超宽带雷达信号发生接收模块、双目相机、imu测量模块、uwb定位终端模块，所述激光雷达、超宽带雷达信号发生接收模块、双目相机、imu测量模块、uwb定位终端模块接收信号，并在接收到瞬间将自身采集到的信号锁存，然后通过总线向所述工控机发送信号，从而使得所有传感器保持时间同步；

13、所述激光雷达、所述超宽带雷达信号发生接收模块、所述双目相机、所述uwb定位终端模块、所述imu测量模块、所述控制模块均与所述工控机相连接，所述工控机的输出与所述数据显示系统的输入端连接；

14、所述步进电机通过同步带驱动所述移动导轨上的滑块运动，所述电机通过串口与计算机相连，所述移动电源为所述电机供电。

15、优选地，在所述数据处理单元对点云数据进行点云去噪、纠正和分割处理中，所述点云去噪方法采用弦高差方法去除噪声点、中值滤波算法；所述纠正方法采用迭代最近点算法，同时利用ki-dtree加快临近点搜索，提升配准速度；所述分割处理采用表面生长分割方法。

16、优选地，一种gps拒止环境下农机田间导航定位方法，其特征在于，包括：

17、s01:根据农田区域的边界的环境，在边界边缘的外侧放置两个移动导轨，通过螺栓将固定杆安装在导轨的滑块上，将4个uwb定位基站安装在所述支架上，同时将高精度的姿态传感器安装在滑块上；

18、s02:在农机车体上第一固定装置和第二固定装置上分别安装激光雷达、超宽带雷达信号发生接收模块、双目相机、uwb定位终端模块、imu测量模块。

19、s03:选择预设位置建立定位坐标系，根据农机车体的作业区域和所述uwb定位基站的通讯范围，随机部署4个所述uwb定位基站并利用全站仪测量标定4个所述uwb定位基站的三维位置坐标，将基站的测量结果输入到所述工控机中，并将4个所述uwb定位基站与所述uwb定位终模块端组网；

20、s04:在所述工控机中计算出位置pdop的值，当pdop值大于2时，在所述工控机中利用优化算法对定位基站的布局方式进行优化，根据优化的结果重新调整4个uwb定位基站；当pdop值小于2时，则将当前的布局作为所述uwb定位基站的最终布局方案；

21、s05:对所述双目相机和所述激光雷达进行初始化，同时标定双目相机的内参，并且对所述双目相机与所述激光雷达之间的外参进行标定，构建所述双目相机与所述激光雷达的坐标系关系；

22、s06:判断当前的天气情况，如果当前的天气为阴天、雨天或者浓雾天气下，启动所述超宽带雷达信号发生接收模块，在所述数据处理单元中根据所述超宽带雷达信号发生接收模块的信号获得农机车体与障碍物的距离；若是晴天，启动所述双目相机和所述激光雷达；

23、若启动所述超宽带雷达信号发生接收模块，在所述数据处理单元中根据所述超宽带雷达信号发生接收模块的信号获得农机车体与障碍物的距离；如果启动所述双目相机和所述激光雷达，所述工控机中构建三维地图模型，实时判断农机车体当前的运行状态，同时获得农机车体前方障碍物的三维位置坐标，并在所述数据处理单元中获得农机车体与障碍物的距离；

24、s07:启动所述uwb定位终端模块和所述imu测量模块，在所述数据处理单元中获得融合uwb和imu的农机车体位置和姿态，将解算结果传输至所述数据显示系统，实时显示定位结果；

25、s08:农机车体按照预设的作业路径进行作业，通过步骤s6实时检测农机车体前方的障碍物存在情况，获得农机车体与障碍物的之间距离，采用人工势场法获得农机车体可通行的作业路径，并将规划的信息传送至所述控制模块，所述控制模块发出控制信号驱动农机车体根据规划的路径运行；

26、s09:通过计数器模块判断农机车体是否完成10次循环作业，若农机车体完成10次作业，控制电机驱动滑块沿着导轨进行移动，根据所述高精度姿态传感器的测量数据和滑块的移动距离，采用航迹推移算法在所述数据处理单元中解算出移动后4个所述uwb定位基站的三维位置坐标，并将移动后基站的三维位置坐标传送给所述工控机中，对农机车体进行连续循环定位。

27、优选地，pdop值由观测矩阵h、加权矩阵w和矩阵m求得，其中观测矩阵h和加权矩阵w分别为：

28、

29、

30、式中(xu,yu,zu)表示uwb系统计算的农机车体位置坐标，(xi,yi,zi)表示第i个uwb定位基站，σi表示第i个uwb定位基站的测距误差方差，其中i＝1,2,3,4。

31、矩阵m的计算过程为：

32、

33、进一步，pdop的计算公式表示为：

34、

35、式中mjj表示矩阵m的对角元素,其中j＝1,2,3。

36、优选地，在所述工控机中利用优化算法对定位基站的布局方式进行优化，具体为：病毒侵入优化算法或者水循环优化算法。

37、优选地，所述双目相机的标定具体的包括：采用基于张正友标定法对左右相机的分别内参进行标定；对左右相机径向畸变系数(k1,k2,k3)和切向畸变系数(p1,p2)进行标定；对左右相机的相对位姿关系进行标定，确定双目基线的长度。

38、优选地，所述激光雷达和所述双目相机的外参标定，具体步骤如下：

39、s11:选用矩形板作为标定板，在矩形板上设置四个大小相同的圆孔，而且在标定板的中心固定一个三面金属反射器，增强所述激光雷达反射能力，其中四个圆孔的圆心连线构成长方形，用卷尺精确测量圆孔的半径和长方形边长；

40、s12：所述双目相机采集标定板圆孔图像，使用sobel检测算子创建边缘图像和基于随机hough变换方法提取图像中圆孔和三面体反射器的信息；所述激光雷达扫描圆孔标定板，提取圆孔和三面体反射器的点云信息，并进行滤波降噪处理，使用随机抽样一致算法重新拟合标定板，获得更加精确的标定板平面；

41、s13：计算平移矩阵和旋转矩阵具体如下：

42、(a)先假设为旋转矩阵为单位矩阵，通过边缘特征匹配，粗略计算出平移矩阵

43、(b)构建所述双目相机和所述激光雷达检测到的特征点集合，利用边缘检测误差和重投影误差优化平移矩阵并计算旋转矩阵

44、

45、式中表示所述激光雷达检测到的特征点所建立的特征点集合，表示所述双目相机检测到的特征点所建立的特征点集合

46、s14：将所述激光雷达点云数据下的三维坐标(xl,yl,zl)转换为所述相机对象的像素坐标(u,v)，如下式：

47、

48、其中，fx,fy表示左右相机的焦距，(u0,v0)表示视觉传感器像素点的原点；zc表示视觉坐标系下的z轴坐标。

49、优选地，把所述激光雷达、所述双目相机、所述imu测量模块、所述超宽带雷达信号发生接收模块获得的数据转换到世界坐标系下，其中将所述双目相机坐标系下的三维坐标(xc,yc,zc)融合到世界坐标系下的三维坐标(xw,yw,zw)的形式如下：

50、

51、式中(xt,yt,zt)表示相机坐标系c的原点到世界坐标系w的原点平移参数；表示相机坐标系与世界坐标系之间的旋转矩阵，其中的计算如下：

52、

53、式中

54、

55、其中θ，γ表示相机坐标系与世界坐标系三个坐标轴x,y,z之间的旋转角。

56、优选地，将所述imu测量模块和所述uwb定位终端模块的定位结果利用布尔莎七参数模型融合到世界坐标系下，融合的误差e写成如下形式：

57、

58、式中xψ，yψ，zψ是uwb和imu融合后的定位结果，e表示imu与uwb定位结果融合的误差矩阵，融合后均值。

59、进一步，构建融合误差的代价函数为：argminet×e，根据最小二乘法原则，代价函数达到最小值时，获得误差参数δθ，δγ，利用误差参数修正imu的定位结果，从而使农机车体获得更高精度的定位结果。

60、在所述数据处理单元中计算uwb定位系统的定位结果与所述imu测量模块的位置估计值的差，若误差小于设定的阈值，则将该误差反馈融合到基于误差状态扩展卡尔曼滤波模型获得最终农机车体的定位结果；若该差值大于设定的阈值，则以uwb定位系统的定位结果作为农机车体的最终定位结果，同时在下一时刻对所述imu测量模块的定位结果进行误差修正，将修正后的结果作为imu测量模块的定位结果。

61、优选地，在所述数据处理单元中采用变分容积卡尔曼滤波对农机车体的定位数据进行平滑处理，获得较稳定的姿态和位置坐标。

62、优选地，在所述数据处理单元中对相邻两时刻的雷达扫描图像进行图像匹配，在所述数据解算单元中获得障碍物与农机车体的距离。当障碍物与农机车体的距离小于设定的阈值时，使用a*算法重新规划农机车体的作业路径，当障碍物与农机车体的距离大于设定的阈值时，农机车体继续按照原定规划路径进行作业。

63、优选地，农机车体与障碍物的之间距离的计算方法为：根据imu和uwb的融合定位结果获得农机车体自身的在世界坐标下的位置坐标为(xa,ya,za)，所述激光雷达和所述双目相机获得障碍物在世界坐标系下的位置坐标为(xf,yf,zf)，障碍物与农机车体之间的距离d可表示为：

64、

65、优选地，障碍物的在世界坐标系下的位置坐标计算过程为：根据所述双目相机解算出的障碍物特征点的像素，在所述数据解算单元中计算出障碍物在所述双目相机坐标系下的三维坐标，再将所述双目相机坐标系的三维坐标转换到世界坐标系下，从而获得障碍物在世界坐标系的三维坐标，其中相机特征点中像素为(u,v)，深度为zs对应的所述双目相机坐标系下的三维坐标(xc,yc,zc)可表示为：

66、

67、本发明实施例提供的一种gps拒止环境下农机车体田间导航定位系统及方法，具有以下有益效果：

68、(1)由定位基站和定位终端构成的uwb定位系统能够实现gps拒止环境下的定位，利用uwb和imu融合定位能够提升农机车体的定位精度；在复杂的环境下，农机车体能获得较高的定位精度。

69、(2)将利用激光雷达和双目相机相结合能构建农机车体的地图模型，同时获得农机车体前方障碍物的位置坐标；超宽带雷达信号发生接收模块达有很强大穿透能力，使得农机车体在雨天、阴天或浓雾天气都能准确的获得农机车体与障碍物之间的距离，使农机车体能有效的避障。

70、(3)农机车体上的uwb定位终端模块超出了定位基站的通讯范围时，uwb定位基站能够沿着导轨移动，避免人工搬移定位基站，利用航迹推移算法能快速计算出移动后定位基站的坐标，实现了农机车体长期连续的作业，提升了农机车体的工作效率，有助于农机车体智能化的实现。