大棚内农机车辆定位方法、系统、计算机设备及存储介质与流程

1.本发明属于定位技术领域，涉及一种大棚内农机车辆定位方法、系统、计算机设备及存储介质。


背景技术：

2.农机自动驾驶是一门涉及到计算机科学、模式识别、电子、通信和控制等多个领域的交叉学科技术。该技术是通过对车辆周边的环境信息进行感知并结合车辆自身的定位，规划出最适合行走和作业的路径，进而控制农机车辆的方向和速度，最终实现农机车辆的自主行走和自主作业。农机自动驾驶技术对于减轻农业劳动强度，提升工作效率以及提升农业产能有着重要的意义，而为了使农机车辆实现自主行走，需要在农机车辆行驶过程中实现精准定位。
3.计算机视觉定位、卫星定位以及惯性导航是自动驾驶领域中计算车辆行驶过程中位姿信息与导航定位的关键技术，在自动驾驶中扮演着至关重要的角色，是成功实现自动驾驶的前提之一。目前，自动驾驶的导航与定位功能大多通过gnss(global navigation satellite system，全球卫星导航系统)加上雷达系统来实现，常见的雷达系统如激光雷达和毫米波雷达等。
4.然而，gnss存在在室内或信号较弱的工作场景下会发生信号丢失现象。同时，尽管雷达系统在高速运动模式下定位精度较为准确，然而对于农机作业时常见的低速或静态工作场景，雷达系统的感知效果较差，无法达到农机自动驾驶所需要的定位精度且其成本十分高昂。这导致gnss加上雷达系统的方式，并不能提供如大棚内等农业场景下，农机车辆自动驾驶作业中的精确定位信息。


技术实现要素：

5.本发明的目的在于克服上述现有技术中，农机车辆定位信息不准确的缺点，提供一种大棚内农机车辆定位方法、系统、计算机设备及存储介质。
6.为达到上述目的，本发明采用以下技术方案予以实现：
7.本发明第一方面，一种大棚内农机车辆定位方法，包括：
8.获取若干连续的工作场景照片以及各工作场景照片的imu数据；
9.根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据；
10.根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标；
11.根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度；
12.根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机
相对坐标；
13.获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标。
14.可选的，所述根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据包括：
15.采用光流法跟踪各工作场景照片中预设位置的特征点，得到特征点跟踪结果，并根据特征点跟踪结果计算各工作场景照片的相机相对位姿数据；
16.采用用中点插值的方法，对各工作场景照片的imu数据进行预积分，得到各工作场景照片的imu相对位姿数据。
17.可选的，所述根据各工作场景照片的相机相对旋转数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标包括：
18.根据各工作场景照片的相机相对旋转数据，采用pnp算法，从第一张工作场景照片向最后一张工作场景照片构建第一三维图，并从最后一张工作场景照片向第一张工作场景照片构建第二三维图，融合第一三维图和第二三维图得到相机位置三维图；将相机位置三维图采用ceres优化器进行优化，提取优化后的相机位置三维图中各工作场景照片的相机相对坐标。
19.可选的，所述根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度包括：
20.根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，计算各工作场景照片的imu偏移矩阵；
21.根据各工作场景照片的相机相对位姿数据、imu数据以及imu偏移矩阵，优化各工作场景照片的imu相对位姿数据，得到各工作场景照片的优化imu相对位姿数据，以及计算各工作场景照片的雅可比矩阵和协方差矩阵；
22.根据各工作场景照片的优化imu相对位姿数据、雅可比矩阵和协方差矩阵，采用ceres优化器，得到各工作场景照片的重力、相机速度和相机尺度。
23.可选的，所述计算各工作场景照片的雅可比矩阵和协方差矩阵包括：
24.设定第一张工作场景照片的雅可比矩阵为单位矩阵，第一张工作场景照片的协方差矩阵为零矩阵；
25.第二张工作场景照片至最后一张工作场景照片的雅可比矩阵分别为相邻上一张工作场景照片的雅可比矩阵乘以imu偏移矩阵；
26.第二张工作场景照片至最后一张工作场景照片的协方差矩阵分别为相邻上一张工作场景照片的协方差矩阵加上一个噪声矩阵；噪声矩阵由当前工作场景照片的imu数据中的加速度和imu噪声的平方构成。
27.可选的，所述根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标包括：
28.根据各工作场景照片的相机尺度，以最小化重投影误差法优化相机位置三维图中各工作场景照片的相机相对坐标，得到各工作场景照片的优化相机相对坐标；
29.固定第一张工作场景照片为世界坐标系，根据各工作场景照片的重力，计算当前的世界坐标系相对于水平坐标系的旋转数据；
30.根据当前的世界坐标系相对于水平坐标系的旋转数据，以及各工作场景照片的相机速度和相机尺度，将各工作场景照片的优化相机相对坐标转换转至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标。
31.可选的，所述根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标包括：
32.获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器构建全局位姿图模型，并通过ceres优化器解算全局位姿图模型，得到工作场景照片的的全局坐标，作为农机车辆定位坐标。
33.本发明第二方面，一种大棚内农机车辆定位系统，包括：
34.数据获取模块，用于获取若干连续的工作场景照片以及各工作场景照片的imu数据；
35.位姿处理模块，用于根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据；
36.相对坐标确定模块，用于根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标；
37.优化模块，用于根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度；
38.坐标转换模块，用于根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标；
39.定位模块，用于获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标。
40.本发明第三方面，一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述大棚内农机车辆定位方法的步骤。
41.本发明第四方面，一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述大棚内农机车辆定位方法的步骤。
42.与现有技术相比，本发明具有以下有益效果：
43.本发明大棚内农机车辆定位方法，根据若干连续的工作场景照片以及各工作场景照片的imu数据，获取各工作场景照片的相机相对位姿数据和imu相对位姿数据，然后根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，实现各工作场景照片的相机相对坐标的获取，然后通过各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度，并基于这些数据将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标；然后基于获取的任一工作场景照片的gnss卫星数据，实现各工作场景照片在
水平坐标系下的相机相对坐标与gnss卫星数据的融合，进而确定农机车辆定位坐标，保证即使由于gnss在室内信号较弱导致部分gnss卫星数据时，仍然能够准确确定农机车辆定位坐标，保证农机车辆定位准确性，且实现成本低。
附图说明
44.图1为本发明实施例的大棚内农机车辆定位方法流程图。
45.图2为本发明实施例的大棚内农机车辆定位系统结构框图。
具体实施方式
46.为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
47.需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
48.下面结合附图对本发明做进一步详细描述：
49.参见图1，本发明一实施例中，提供一种大棚内农机车辆定位方法，具体为融合卫星视觉与惯性的大棚内农机车辆定位方法，有效解决目前大棚内农机车辆自动驾驶中定位功能效果差且成本高的问题。
50.具体的，该大棚内农机车辆定位方法包括以下步骤：
51.s1：获取若干连续的工作场景照片以及各工作场景照片的imu数据。
52.s2：根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据。
53.s3：根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标。
54.s4：根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度。
55.s5：根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标。
56.s6：获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标。
57.其中，工作场景照片为搭载在农机车辆上的相机在农机车辆工作时所拍摄的照片。各工作场景照片的imu数据具体为在各工作场景照片的拍摄时间时，由搭载在农机车辆上的imu(惯性测量单元)所采集的数据。各工作场景照片的相机相对位姿数据和imu相对位姿数据，均为在各工作场景照片的拍摄时间时的相机相对位姿数据和imu相对位姿数据。
58.综上所述，本发明大棚内农机车辆定位方法，根据若干连续的工作场景照片以及各工作场景照片的imu数据，获取各工作场景照片的相机相对位姿数据和imu相对位姿数据，然后根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，实现各工作场景照片的相机相对坐标的获取，然后通过各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度，并基于这些数据将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标；然后基于获取的任一工作场景照片的gnss卫星数据，实现各工作场景照片在水平坐标系下的相机相对坐标与gnss卫星数据的融合，进而确定农机车辆定位坐标，保证即使由于gnss在室内信号较弱导致部分gnss卫星数据时，仍然能够准确确定农机车辆定位坐标，保证农机车辆定位准确性，且实现成本低。
59.在一种可能的实施方式中，所述根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据包括：采用光流法跟踪各工作场景照片中预设位置的特征点，得到特征点跟踪结果，并根据特征点跟踪结果计算各工作场景照片的相机相对位姿数据；采用用中点插值的方法，对各工作场景照片的imu数据进行预积分，得到各工作场景照片的imu相对位姿数据。
60.具体的，本实施方式中，采用光流法跟踪各工作场景照片中预设位置的特征点时，跟踪各工作场景照片的8个预设位置的特征点，并采用8点法计算各工作场景照片的相机相对位姿数据。
61.在一种可能的实施方式中，所述根据各工作场景照片的相机相对旋转数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标包括：根据各工作场景照片的相机相对旋转数据，采用pnp算法，从第一张工作场景照片向最后一张工作场景照片构建第一三维图，并从最后一张工作场景照片向第一张工作场景照片构建第二三维图，融合第一三维图和第二三维图得到相机位置三维图；将相机位置三维图采用ceres优化器进行优化，提取优化后的相机位置三维图中各工作场景照片的相机相对坐标。
62.其中，pnp(perspective-n-point)算法是求解3d到2d点的对应方法。它描述了当知道n个3d空间点及其位置，如何估计相机的位姿。如果两张图像中的一张特征点3d位置已知，那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。ceres优化器运用在slam前端或后端，设置对相机位姿和路标点进行非线性寻优的ceres算法。
63.在一种可能的实施方式中，所述根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度包括：根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，计算各工作场景照片的imu偏移矩阵；根据各工作场景照片的相机相对位姿数据、imu数据以及imu偏移矩阵，优化各工作场景照片的imu相对位姿数据，得到各工作场景照片的优化imu相对位姿数据，以及计算各工作场景照片的雅可比矩阵和协方差矩阵；根据各工作场景照片的优化imu相对位姿数据、雅可比矩阵和协
方差矩阵，采用ceres优化器，得到各工作场景照片的重力、相机速度和相机尺度。
64.其中，通过ceres优化器优化各工作场景照片的重力、相机速度和相机尺度时，以重力的方向作为初值，限定模为9.8，进一步优化重力，对重力模进行切线方向上的调整，因为是线性的，所以手动调整4次重力的方向，调整时采用高斯牛顿方法处理，然后基于重力优化结果用高斯牛顿方法进一步优化相机速度和相机尺度，相机尺度大于零，则认为优化成功。
65.其中，计算各工作场景照片的雅可比矩阵和协方差矩阵包括：设定第一张工作场景照片的雅可比矩阵为单位矩阵，第一张工作场景照片的协方差矩阵为零矩阵；第二张工作场景照片至最后一张工作场景照片的雅可比矩阵分别为相邻上一张工作场景照片的雅可比矩阵乘以imu偏移矩阵；第二张工作场景照片至最后一张工作场景照片的协方差矩阵分别为相邻上一张工作场景照片的协方差矩阵加上一个噪声矩阵；噪声矩阵由当前工作场景照片的imu数据中的加速度和imu噪声的平方构成。
66.在一种可能的实施方式中，所述根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标包括：根据各工作场景照片的相机尺度，以最小化重投影误差法优化相机位置三维图中各工作场景照片的相机相对坐标，得到各工作场景照片的优化相机相对坐标；固定第一张工作场景照片为世界坐标系，根据各工作场景照片的重力，计算当前的世界坐标系相对于水平坐标系的旋转数据；根据当前的世界坐标系相对于水平坐标系的旋转数据，以及各工作场景照片的相机速度和相机尺度，将各工作场景照片的优化相机相对坐标转换转至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标。
67.具体的，在对若干连续的工作场景照片进行处理时，可以采用滑动窗口的形式进行处理，上述处理过程可认为是一个窗口内的连续工作场景照片的处理。
68.在一种可能的实施方式中，所述根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标包括：获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器构建全局位姿图模型，并通过ceres优化器解算全局位姿图模型，得到工作场景照片的的全局坐标，作为农机车辆定位坐标。
69.具体的，gnss卫星数据可通过搭载在农机车辆上的gnss全局传感器得到。将得到的工作场景照片的水平坐标系下的坐标与gnss卫星数据相融合并由ceres优化器构建全局位姿图模型，通过解算该全局图优化模型后得到局部准确且无全局漂移的全局统一坐标，实现农机在自主行驶过程中的精确定位。
70.下述为本发明的装置实施例，可以用于执行本发明方法实施例。对于装置实施例中未披露的细节，请参照本发明方法实施例。
71.参见图2，本发明再一实施例中，提供一种大棚内农机车辆定位系统，能够用于实现上述的大棚内农机车辆定位方法，具体的，该大棚内农机车辆定位系统包括数据获取模块、位姿处理模块、相对坐标确定模块、优化模块、坐标转换模块以及定位模块。
72.其中，数据获取模块用于获取若干连续的工作场景照片以及各工作场景照片的
imu数据；位姿处理模块用于根据若干连续的工作场景照片以及各工作场景照片的imu数据，得到各工作场景照片的相机相对位姿数据和imu相对位姿数据；相对坐标确定模块用于根据各工作场景照片的相机相对位姿数据，构建相机位置三维图，并提取相机位置三维图中各工作场景照片的相机相对坐标；优化模块用于根据各工作场景照片的相机相对位姿数据和imu相对位姿数据，得到各工作场景照片的重力、相机速度和相机尺度；坐标转换模块用于根据各工作场景照片的重力、相机速度和相机尺度，将相机位置三维图中各工作场景照片的相机相对坐标转换至水平坐标系，得到各工作场景照片在水平坐标系下的相机相对坐标；定位模块用于获取任一工作场景照片的gnss卫星数据，并根据各工作场景照片在水平坐标系下的相机相对坐标和任一工作场景照片的gnss卫星数据，通过ceres优化器，得到农机车辆定位坐标。
73.前述的大棚内农机车辆定位方法的实施例涉及的各步骤的所有相关内容均可以援引到本发明施例中的大棚内农机车辆定位系统所对应的功能模块的功能描述，在此不再赘述。本发明实施例中对模块的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，另外，在本发明各个实施例中的各功能模块可以集成在一个处理器中，也可以是单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
74.本发明再一个实施例中，提供了一种计算机设备，该计算机设备包括处理器以及存储器，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于执行所述计算机存储介质存储的程序指令。处理器可能是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其是终端的计算核心以及控制核心，其适于实现一条或一条以上指令，具体适于加载并执行计算机存储介质内一条或一条以上指令从而实现相应方法流程或相应功能；本发明实施例所述的处理器可以用于上述大棚内农机车辆定位方法的操作。
75.本发明再一个实施例中，本发明还提供了一种存储介质，具体为计算机可读存储介质(memory)，所述计算机可读存储介质是计算机设备中的记忆设备，用于存放程序和数据。可以理解的是，此处的计算机可读存储介质既可以包括计算机设备中的内置存储介质，当然也可以包括计算机设备所支持的扩展存储介质。计算机可读存储介质提供存储空间，该存储空间存储了终端的操作系统。并且，在该存储空间中还存放了适于被处理器加载并执行的一条或一条以上的指令，这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是，此处的计算机可读存储介质可以是高速ram存储器，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。可由处理器加载并执行计算机可读存储介质中存放的一条或一条以上指令，以实现上述实施例中有关大棚内农机车辆定位方法的相应步骤。
76.本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机
可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
77.本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
78.这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
79.这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
80.最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。