CN117128951B - 适用于自动驾驶农机的多传感器融合导航定位系统及方法 - Google Patents

适用于自动驾驶农机的多传感器融合导航定位系统及方法 Download PDF

Info

Publication number
CN117128951B
CN117128951B CN202311402731.4A CN202311402731A CN117128951B CN 117128951 B CN117128951 B CN 117128951B CN 202311402731 A CN202311402731 A CN 202311402731A CN 117128951 B CN117128951 B CN 117128951B
Authority
CN
China
Prior art keywords
coordinate system
information
agricultural machine
intelligent agricultural
positioning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202311402731.4A
Other languages
English (en)
Other versions
CN117128951A (zh
Inventor
成芳
李晓婉
沈朋礼
呼宇航
孙文硕
卢晓春
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National Time Service Center of CAS
Original Assignee
National Time Service Center of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National Time Service Center of CAS filed Critical National Time Service Center of CAS
Priority to CN202311402731.4A priority Critical patent/CN117128951B/zh
Publication of CN117128951A publication Critical patent/CN117128951A/zh
Application granted granted Critical
Publication of CN117128951B publication Critical patent/CN117128951B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Abstract

本发明涉及一种适用于自动驾驶农机的多传感器融合导航定位系统及方法，该方法包括：实时获取智能农机的视觉信息、惯性导航信息和GNSS定位信息；利用视觉信息和惯性导航信息，得到智能农机的第一状态信息；利用GNSS定位信息对第一状态信息进行优化，得到智能农机的第二状态信息；其中，若GNSS的定位信号正常，则GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；利用第二状态信息实现智能农机的导航定位。本发明利用卫星导航的绝对定位性能，弥补惯性和视觉导航的缺点，同时利用视觉和惯性导航的相对高精度弥补卫星导航受干扰时丢失信号的缺点，能够有效的提高农业生产效率。

Description

适用于自动驾驶农机的多传感器融合导航定位系统及方法
技术领域
本发明属于多传感器导航定位技术领域，具体涉及一种适用于自动驾驶农机的多传感器融合导航定位系统及方法。
背景技术
定位技术的发展使得世界各国的农业自动化水平都得到了显著提高，特别是在精准农业领域，利用不同传感器实现智能农机设备的自动驾驶和精准定位已成为农业生产者提高生产效率的重要手段。精准农业技术被认为是21世纪农业科技发展的前沿，是科技含量最高、集成综合性最强的现代农业生产管理技术之一。精准农业技术是根据空间变异，定位、定时、定量的实施一套现代化农事操作技术与管理的系统，是信息技术与农业生产全面结合的一种新型农业技术。
卫星导航技术是精准农业技术的基本组成之一，使智能农机实现自动行驶，智能农机作业前只需要设定好参数后，导航系统便引导智能农机进入自动作业模式，开始进行耕作。在智能农机自动导航的过程中，农田的环境恶劣且复杂，大型的农田里可能会存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等，这些因素都为无人驾驶智能农机的实现提出了新的挑战。
使用卫星导航技术可以实现智能农机在农田里自动行走，但是智能农机无法识别智能农机前方的障碍物，即智能农机无法感应农田环境。此外，国内现有智能农机的定位技术手段为普通单点定位（SPP）和局域实时动态差分技术（RTK）。SPP只能实现米级定位，不能满足智能农机的定位精度需求。RTK是一种局域定位技术，虽然定位精度较高，但是服务范围较小，不能满足大范围高精度定位的需求，同时网络难覆盖偏远地区，需要卫星广播通讯，非人口密集中心城市，定位所需的参考站大多分布稀疏。
近年来，多传感器导航系统及相关技术得到了迅速的发展和应用，例如，全球定位系统（GNSS）与惯性导航系统（INS）相结合的导航方式，但是在精准农业领域，由于成本问题以及算法处理问题等，该组合导航还没有得到广泛推广。随着对智能农机的导航定位精度要求和环境感知能力的不断提高，现有的导航方式受智能农机作业环境等因素影响所产生的误差导致其精准度无法满足需求。
发明内容
为了解决现有技术中存在的上述问题，本发明提供了一种适用于自动驾驶农机的多传感器融合导航定位系统及方法。本发明要解决的技术问题通过以下技术方案实现：
本发明提供了一种适用于自动驾驶农机的多传感器融合导航定位方法，包括：
实时获取智能农机的视觉信息、惯性导航信息和GNSS定位信息，其中，所述GNSS定位信息是基于非差模式的PPP-RTK定位技术得到的；
利用所述视觉信息和所述惯性导航信息，得到所述智能农机的第一状态信息；
利用所述GNSS定位信息对所述第一状态信息进行优化，得到所述智能农机的第二状态信息；
其中，若GNSS的定位信号正常，则所述GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则所述GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；
利用所述第二状态信息实现所述智能农机的导航定位。
本发明提供了一种适用于自动驾驶农机的多传感器融合导航定位系统，包括：
GNSS接收机，用于实时获取智能农机的GNSS定位信息，其中，所述GNSS接收机设置有卫星通信模块和地面通信模块，以实现卫星和地面之间的4G/5G移动通信、蓝牙通信或WIFI通信；
IMU装置，用于实时获取智能农机的惯性导航信息；
双目视觉相机，用于实时获取智能农机的视觉信息；
信息采集与处理装置，用于接收所述智能农机的视觉信息、惯性导航信息和GNSS定位信息，并利用所述视觉信息和所述惯性导航信息，得到所述智能农机的第一状态信息；利用所述GNSS定位信息对所述第一状态信息进行优化，得到所述智能农机的第二状态信息；其中，若GNSS的定位信号正常，则所述GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则所述GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；
用户端，用于接收所述智能农机的第二状态信息，并利用所述第二状态信息实现所述智能农机的导航定位。
与现有技术相比，本发明的有益效果在于：
1. 本发明的适用于自动驾驶农机的多传感器融合导航定位方法，利用卫星导航、惯性导航、视觉导航相结合的多传感器导航方式实现智能农机的导航定位，能够充分发挥上述三种导航的优势，利用卫星导航的长期稳定性与高精度的绝对定位性能，来弥补惯性导航的误差随时间传播增大和视觉导航受外界运动物体干扰的缺点，同时利用视觉导航、惯性导航的相对高精度来弥补卫星导航受干扰时误差增大或受遮挡时丢失信号的缺点。能够有效的提高农业生产效率。
2. 本发明的适用于自动驾驶农机的多传感器融合导航定位系统，通过双目视觉相机和惯性测量装置来弥补GNSS定位信号受到干扰时的定位精度，利用多传感器融合算法对双目视觉相机、IMU装置、GNSS接收机获取的智能农机的信息进行融合定位，提高智能农机的定位精度和定位鲁棒性。当智能农机在等野外复杂环境下，GNSS定位信号频繁受干扰时，也可以有效解决定位精度下降问题，从而为以低成本的系统为智能农机提供高精度实时的定位信息和三维地图信息。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
图1是本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位方法的示意图；
图2是本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位方法的流程图；
图3是本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位系统的结构框图。
具体实施方式
为了进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及具体实施方式，对依据本发明提出的一种适用于自动驾驶农机的多传感器融合导航定位系统及方法进行详细说明。
有关本发明的前述及其他技术内容、特点及功效，在以下配合附图的具体实施方式详细说明中即可清楚地呈现。通过具体实施方式的说明，可对本发明为达成预定目的所采取的技术手段及功效进行更加深入且具体地了解，然而所附附图仅是提供参考与说明之用，并非用来对本发明的技术方案加以限制。
第一方面，本发明提供了一种适用于自动驾驶农机的多传感器融合导航定位方法，请结合参见图1和图2，图1是本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位方法的示意图；图2是本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位方法的流程图。如图所示，本是实施例的适用于自动驾驶农机的多传感器融合导航定位方法，包括：
S1：实时获取智能农机的视觉信息、惯性导航（IMU）信息和GNSS定位信息；
可选地，利用双目视觉相机获取智能农机所处场景的视觉信息，双目视觉相机包括左相机和右相机，视觉信息包括左相机的视觉信息和右相机的视觉信息。利用IMU装置获取智能农机的IMU信息，IMU装置包括加速度计和陀螺仪，IMU信息包括智能农机在三个方向上的角速度和加速度信息。利用GNSS接收机获取智能农机的GNSS定位信息，即，RTK解算的高精度绝对定位结果。
在本实施例中，GNSS定位技术方面采用了广域高精度定位技术，GNSS定位信息是基于非差模式的PPP-RTK（精密单点定位技术-局域实时动态差分技术）定位技术得到的，PPP-RTK定位技术将广域高精度与区域高精度定位技术结合，能够同时兼顾PPP技术与网络RTK技术各自的优势，利用各类实时高精度改正产品，实现对全球PPP与区域RTK在技术上的统一和服务上的无缝连接。
可选地，GNSS接收机设置有卫星通信模块和地面通信模块，以实现卫星和地面之间的4G/5G移动通信、蓝牙通信或WIFI通信，在本实施例中，多种通信方式保证能够及时获取外部的增强定位信息，即GNSS定位信息，适应多种作业环境下的定位。
S2：利用视觉信息和惯性导航信息，得到智能农机的第一状态信息；
在一个可选的实施例中，S2包括：
利用视觉信息和惯性导航信息进行视觉惯性导航定位，得到智能农机的第一状态信息，智能农机的第一状态信息表示为：，其中，/>为相对位置信息，/>为速度信息，/>为初始化时相机坐标系到IMU坐标系的旋转四元数，/>为加速度、/>为角加速度，/>为局部坐标系，/>为/>时刻的IMU坐标系，/>为IMU坐标系，/>为时间。
在本实施例中，利用视觉信息和惯性信息进行视觉惯性导航定位可以确定智能农机的相对位置、速度和姿态信息（加速度和角加速度）。需要说明的是利用视觉信息和惯性信息进行视觉惯性导航定位获取的智能农机的位置为在IMU坐标系下的相对位置，即实现了智能农机的相对定位。
S3：利用GNSS定位信息对第一状态信息进行优化，得到智能农机的第二状态信息；
其中，若GNSS的定位信号正常，则GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；
在一个可选的实施例中，S3包括：
步骤1：根据GNSS定位信息得到智能农机的全球位置信息，根据全球位置信息和第一状态信息中的相对位置信息，得到全球坐标系和局部坐标系的转换偏航角；
此时，智能农机状态则变为，/>为全球坐标系和局部坐标系的转换偏航角，对于局部坐标系，旋转矩阵/>可表示为：，/>为ENU（东北天）坐标系。
步骤2：利用因子图优化算法，通过滑动窗口对GNSS定位信息和第一状态信息进行实时融合，更新第一状态信息中的相对位置信息、速度信息、加速度和角加速度，得到智能农机的第二状态信息。
在一个可选的实施例中，随着智能农机的移动，GNSS接收机、IMU装置、双目视觉相机继续获取智能农机的对应的GNSS定位信息、惯性导航信息和，视觉信息，然后利用接收到的不断更新的数据构建智能农机的运动模型，进而利用该运动模型建立因子图模型，通过滑动窗口从而对多传感器的数据进行实时融合不断更新位置、速度和姿态信息。
在本实施例中，更新的位置信息为在全球坐标系下的更精确的绝对位置，即实现了智能农机的绝对定位，第二状态信息包括绝对位置信息、速度信息、加速度和角加速度。
S4：利用第二状态信息实现智能农机的导航定位。
在本实施例中，利用第二状态信息中更精确的绝对位置信息实现智能农机的导航定位。
在一个可选的实施例中，本实施例的适用于自动驾驶农机的多传感器融合导航定位方法，还包括：
S5：根据视觉信息和第二状态信息，得到智能农机采集的图像的特征点在全球坐标系下的三维坐标；
可选地，智能农机启动后，开始采集智能农机所处场景的视觉信息，即，智能农机所处场景的图像，通过对图像进行特征提取和特征匹配，最终获取图像特征点的三维坐标信息，此时获取的特征点的三维坐标信息是基于局部坐标系下的。
具体地，可通过以下具体步骤获取特征点的三维坐标信息，包括：
步骤①：对左右相机进行标定，可选地，采用张正友标定法对相机进行标定，获得左右相机的内外参数；
步骤②：对左右相机图像进行校正，可选地，利用标定好的左右相机所获得的内外参数，对图像进行畸变校正和立体校正；
步骤③：对图像进行立体匹配，可选地，利用SGBM（半全局匹配）算法对图像进行立体匹配，最终获得视差图；
步骤④：获得特征点的三维坐标信息，可选地，利用视差图和左右相机的内参数，获得深度图像，根据相机模型和标定相机获得的参数，获得特征点的三维坐标信息。
在本实施例中，获取特征点的三维坐标信息之后，可以根据相机模型、相机参数和第二状态信息中的绝对位置信息获得特征点在全球坐标系下的三维坐标。
可选地，将特征点在全球坐标系下的三维坐标发送至用户端保存，可以利用其进行智能农机环境工作环境的三维重建，获得全球坐标系下的三维地图，在智能农机运行过程中，智能农机的位置可以实时显示在三维地图上，方便用户查看。
在其他实施例中，也可以将智能农机运行历史路线保存在用户端，方便用户查看。
S6：根据图像的特征点在全球坐标系下的三维坐标与智能农机的第二状态信息，获取智能农机与障碍物之间的绝对距离；
在本实施例中，通过计算特征点在全球坐标系下的三维坐标与智能农机的绝对位置之间的距离，得到取智能农机与障碍物之间的绝对距离。
S7：若绝对距离小于预设的安全距离，则生成紧急暂停指令，并将紧急暂停指令发送至智能农机，以停止运行智能农机，否则，智能农机继续运行。
在本实施例中，安全距离是根据智能农机和环境人为进行设定的。
在其他实施例中，还可以提前对智能农机的行驶范围进行预设，当智能农机行驶出预设的范围时，进行信息反馈。
需要说明的是，在利用GNSS定位信息时，可以对其进行大地坐标系到空间直角坐标系的相互转换。大地坐标系下的坐标在位置数据上传至用户端时，更适用于电子地图的实时显示功能，便于用户查看；空间直角坐标系下的坐标有利于在系统中进行解算。
进一步地，对如何利用因子图优化算法，更新第一状态信息中的相对位置信息、速度信息、加速度和角加速度，得到智能农机的第二状态信息的过程进行详细说明。可选地，包括以下步骤：
步骤2.1：构建优化问题并建立优化问题的约束；
构建的优化问题为：
（1）；
其中，为滑动窗口下的智能农机的状态，/>为/>的最优解，，/>，/>，/>为滑动窗口的大小，/>为图像的特征点个数，/>为第/>个特征点，/>为全球坐标系和局部坐标系的转换偏航角，/>表示系统封装的先验边缘化信息，/>为先验残差，/>为先验的海森矩阵/>为智能农机中/>个独立传感器测量的集合，/>为范数运算，/>为表示每个独立传感器测量的残差函数，/>为马氏范数；
在本实施例中，假设所有传感器（即，双目视觉相机、IMU装置、GNSS接收机）测量彼此独立，并且每个测量的噪声是零均值高斯分布的，则MAP（最大后验）问题可以进一步转换为最小化一系列成本之和的问题，每个成本对应于一个特定的测量。对于上述优化问题，可以将其分解为与状态和测量相关的单个因素。
优化问题的约束包括：
基于视觉信息的约束：利用图像的特征点的重投影误差对智能农机的状态进行约束；
其中，特征点的投影过程建模为：
（2）；
其中，为图像平面中的特征坐标，/>为相机坐标系下/>方向的值，为相机坐标系下/>方向的值，/>为转置，/>为相机投影函数，/>为相机坐标系，/>为IMU坐标系向相机坐标系转换的旋转矩阵，/>为局部坐标系向IMU坐标系转换的旋转矩阵，为局部坐标系向IMU坐标系转换的平移向量，/>为IMU坐标系向相机坐标系转换的平移向量，/>为相机在局部坐标系中的位置，/>为测量噪声；
在本实施例中，如果一个特征可以同时被两帧图像看到，那么，该特征点相对于这两帧图像的残差，即，特征点的重投影误差表示为：
（3）；
为两帧图像之间的残差，/>为特征/>的测量，/>为特征/>在/>时刻的相机坐标系的坐标，/>为特征/>，/>为局部坐标系向/>时刻的IMU坐标系转换的旋转矩阵，为/>时刻的IMU坐标系向局部坐标系转换的旋转矩阵，/>为相机坐标系向IMU坐标系转换的旋转矩阵，/>为特征/>的深度，/>为相机坐标系向IMU坐标系转换的平移向量，/>为/>时刻的IMU坐标系向局部坐标系转换的平移向量，/>为局部坐标系向/>时刻的IMU坐标系转换的平移向量。
基于惯性导航信息的约束：利用IMU测量的偏差对智能农机的状态进行约束；
在本实施例中，惯性因子中包含的测量值是平台的有偏差的、有线性噪声的加速度和角速度。在实际中，IMU测量数据的频率高于视觉数据的频率，通常通过预积分进行处理，残差与系统状态相关，预集成的IMU测量可以公式化为：
（4）；
其中，为IMU测量的积分残差，/>为IMU装置的/>时刻与/>时刻之间的测量，/>为/>时刻的IMU坐标系，/>为封装了/>帧和/>帧之间的相对位置和速度，用于在没有初始位置、速度剖面的情况下构建给定IMU测量偏差的速度剖面，/>为建模三维欧几里得空间中的相对旋转误差，/>是加速度计偏差，/>是陀螺仪偏差，/>为残差，为重力加速度偏差，/>为在局部坐标系下的动力加速度，/>为时间差，/>为/>时刻与/>时刻之间相对位置，/>为/>时刻与/>时刻之间的速度，/>为/>时刻与时刻之间的旋转，/>为返回四元数虚部，/>为克罗内克积。
基于GNSS定位信息的约束：利用智能农机的全球位置信息与根据视觉信息和惯性导航信息获取的局部坐标信息之间的残差，对智能农机的状态进行约束；
其中，该残差表示为：
（5）；
其中，为位置残差，/>为GNSS接收机在/>时刻下的测量值，/>为智能农机的全球位置信息，/>为全球坐标系，/>为ENU坐标系，/>为局部坐标系到ENU坐标系之间的旋转矩阵，/>为ENU坐标系到全球坐标系的旋转矩阵，/>表示在智能农机正常运行下，将根据GNSS定位信息获取的第一个有效坐标作为锚点的坐标；
步骤2.2：在建立的约束下，对优化问题进行求解，得到智能农机的第二状态信息。
需要说明的是，在本实施例中，若GNSS的定位信号异常，即GNSS接收机发生故障时，不能获取实时的智能农机的全球位置信息时，则智能农机状态退化成，/>为最新的全球坐标系和局部坐标系的转换偏航角，即，根据GNSS的定位信号异常前最后一次获取的GNSS定位信息得到的智能农机的全球位置信息与此刻的第一状态信息中的相对位置信息得到的。当GNSS接收机发生故障时，此刻，智能农机仍然可以根据惯性信息和视觉信息得到智能农机的绝对坐标。
值得注意的是，在其他实施例中，若视觉信息或惯性导航信息失效时，也可利用其余的两种信息对智能农机进行定位。
示例性地，当GNSS的定位信号异常时，利用视觉信息和惯性导航信息进行定位，主要利用的是公式（1）-（4）进行计算；当视觉信息失效时，利用GNSS定位信息和惯性导航信息进行定位，主要利用的是公式（1）、（4）和（5）进行计算；当惯性导航信息失效时，利用GNSS定位信息和视觉信息进行定位，主要利用公式（1）-（3）和（5）进行计算。
本发明实施例的适用于自动驾驶农机的多传感器融合导航定位方法，利用卫星导航、惯性导航、视觉导航相结合的多传感器导航方式实现智能农机的导航定位，能够充分发挥上述三种导航的优势，利用卫星导航的长期稳定性与高精度的绝对定位性能，来弥补惯性导航的误差随时间传播增大和视觉导航受外界运动物体干扰的缺点，同时利用视觉导航、惯性导航的相对高精度来弥补卫星导航受干扰时误差增大或受遮挡时丢失信号的缺点。能够有效的提高农业生产效率。
第二方面，本发明提供了一种适用于自动驾驶农机的多传感器融合导航定位系统，请参见图3所示的本发明实施例提供的一种适用于自动驾驶农机的多传感器融合导航定位系统的结构框图，如图所示，本实施例的适用于自动驾驶农机的多传感器融合导航定位系统，包括：GNSS接收机、IMU装置、双目视觉相机、信息采集与处理装置和用户端。
其中，GNSS接收机用于实时获取智能农机的GNSS定位信息，其中， GNSS接收机设置有卫星通信模块和地面通信模块，以实现卫星和地面之间的4G/5G移动通信、蓝牙通信或WIFI通信；IMU装置用于实时获取智能农机的惯性导航信息；双目视觉相机用于实时获取智能农机的视觉信息；信息采集与处理装置用于接收智能农机的视觉信息、惯性导航信息和GNSS定位信息，并利用视觉信息和惯性导航信息，得到智能农机的第一状态信息；利用GNSS定位信息对第一状态信息进行优化，得到智能农机的第二状态信息；其中，若GNSS的定位信号正常，则GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；用户端用于接收智能农机的第二状态信息，并利用第二状态信息实现智能农机的导航定位。
进一步地，在本实施例中，信息采集与处理装置还用于根据视觉信息和第二状态信息，得到图像的特征点在全球坐标系下的三维坐标；根据图像的特征点在全球坐标系下的三维坐标与智能农机的第二状态信息，获取智能农机与障碍物之间的绝对距离；若绝对距离小于预设的安全距离，则生成紧急暂停指令，并将紧急暂停指令发送至智能农机，以使智能农机的自动驾驶设备根据接收的所述紧急暂停指令，控制所述智能农机停止运行。
关于该适用于自动驾驶农机的多传感器融合导航定位系统的具体内容以及相应的有益效果，请参见第一方面提供的适用于自动驾驶农机的多传感器融合导航定位方法的相关内容，在此不做赘述。
本发明实施例的适用于自动驾驶农机的多传感器融合导航定位系统，通过双目视觉相机和惯性测量装置来弥补GNSS定位信号受到干扰时的定位精度，利用多传感器融合算法对双目视觉相机、IMU装置、GNSS接收机获取的智能农机的信息进行融合定位，提高智能农机的定位精度和定位鲁棒性。当智能农机在等野外复杂环境下，GNSS定位信号频繁受干扰时，也可以有效解决定位精度下降问题，从而为以低成本的系统为智能农机提供高精度实时的定位信息和三维地图信息。
应当说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的物品或者设备中还存在另外的相同要素。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (4)

1.一种适用于自动驾驶农机的多传感器融合导航定位方法，其特征在于，包括：
实时获取智能农机的视觉信息、惯性导航信息和GNSS定位信息，其中，所述GNSS定位信息是基于非差模式的PPP-RTK定位技术得到的；
利用所述视觉信息和所述惯性导航信息，得到所述智能农机的第一状态信息；包括：利用所述视觉信息和所述惯性导航信息进行视觉惯性导航定位，得到所述智能农机的第一状态信息，所述智能农机的第一状态信息表示为：
其中，为相对位置信息，/>为速度信息，/>为初始化时相机坐标系到IMU坐标系的旋转四元数，ba为加速度、bw为角加速度，w为局部坐标系，bk为k时刻的IMU坐标系，b为IMU坐标系，k为时间；
利用所述GNSS定位信息对所述第一状态信息进行优化，得到所述智能农机的第二状态信息；包括：
步骤1：根据所述GNSS定位信息得到所述智能农机的全球位置信息，根据所述全球位置信息和所述第一状态信息中的相对位置信息，得到全球坐标系和局部坐标系的转换偏航角；
步骤2：利用因子图优化算法，通过滑动窗口对所述GNSS定位信息和所述第一状态信息进行实时融合，更新所述第一状态信息中的相对位置信息、速度信息、加速度和角加速度，得到所述智能农机的第二状态信息；包括：
步骤2.1：构建优化问题并建立所述优化问题的约束；
所述优化问题为：
其中，为滑动窗口下的智能农机的状态，/>为/>的最优解，n为滑动窗口的大小，m为图像的特征点个数，ρm为第m个特征点，/>为全球坐标系和局部坐标系的转换偏航角，{rp,Hp}表示系统封装的先验边缘化信息，rp为先验残差，Hp为先验的海森矩阵zi为智能农机中o个独立传感器测量的集合，||·||2为范数运算，r(·)为表示每个独立传感器测量的残差函数，/>为马氏范数；
所述约束包括：
基于所述视觉信息的约束：利用图像的特征点的重投影误差对所述智能农机的状态进行约束；
基于所述惯性导航信息的约束：利用IMU测量的偏差对所述智能农机的状态进行约束；
基于所述GNSS定位信息的约束：利用所述智能农机的全球位置信息与根据所述视觉信息和所述惯性导航信息获取的局部坐标信息之间的残差，对所述智能农机的状态进行约束；
步骤2.2：在建立的约束下，对所述优化问题进行求解，得到所述智能农机的第二状态信息；
其中，在所述基于所述GNSS定位信息的约束中，所述残差表示为：
其中，rp(·)为位置残差，为GNSS接收机在k时刻下的测量值，/>为智能农机的全球位置信息，e为全球坐标系，n为ENU坐标系，/>为局部坐标系到ENU坐标系之间的旋转矩阵，/>为ENU坐标系到全球坐标系的旋转矩阵，/>表示在智能农机正常运行下，将根据GNSS定位信息获取的第一个有效坐标作为锚点的坐标；
其中，若GNSS的定位信号正常，则所述GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则所述GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；
其中，在所述基于所述视觉信息的约束中，所述特征点的投影过程建模为：
其中，为图像平面中的特征坐标，u为相机坐标系下x方向的值，v为相机坐标系下y方向的值，T为转置，πc为相机投影函数，c为相机坐标系，/>为IMU坐标系向相机坐标系转换的旋转矩阵，/>为局部坐标系向IMU坐标系转换的旋转矩阵，/>为局部坐标系向IMU坐标系转换的平移向量，/>为IMU坐标系向相机坐标系转换的平移向量，xw为相机在局部坐标系中的位置，nc为测量噪声；
所述特征点的重投影误差表示为：
其中，为两帧图像之间的残差，/>为特征l的测量，/>为特征l在j时刻的相机坐标系的坐标，/>为特征l，/>为局部坐标系向j时刻的IMU坐标系转换的旋转矩阵，/>为i时刻的IMU坐标系向局部坐标系转换的旋转矩阵，/>为相机坐标系向IMU坐标系转换的旋转矩阵，ρl为特征l的深度，/>为相机坐标系向IMU坐标系转换的平移向量，/>为i时刻的IMU坐标系向局部坐标系转换的平移向量，/>为局部坐标系向j时刻的IMU坐标系转换的平移向量；
利用所述第二状态信息实现所述智能农机的导航定位。
2.根据权利要求1所述的适用于自动驾驶农机的多传感器融合导航定位方法，其特征在于，还包括：
根据所述视觉信息和所述第二状态信息，得到智能农机采集的图像的特征点在全球坐标系下的三维坐标；
根据所述图像的特征点在全球坐标系下的三维坐标与所述智能农机的第二状态信息，获取所述智能农机与障碍物之间的绝对距离；
若所述绝对距离小于预设的安全距离，则生成紧急暂停指令，并将所述紧急暂停指令发送至所述智能农机，以停止运行所述智能农机，否则，所述智能农机继续运行。
3.一种适用于自动驾驶农机的多传感器融合导航定位系统，其特征在于，包括：
GNSS接收机，用于实时获取智能农机的GNSS定位信息，其中，所述GNSS接收机设置有卫星通信模块和地面通信模块，以实现卫星和地面之间的4G/5G移动通信、蓝牙通信或WIFI通信；
IMU装置，用于实时获取智能农机的惯性导航信息；
双目视觉相机，用于实时获取智能农机的视觉信息；
信息采集与处理装置，用于接收所述智能农机的视觉信息、惯性导航信息和GNSS定位信息，并利用所述视觉信息和所述惯性导航信息，得到所述智能农机的第一状态信息，所述智能农机的第一状态信息表示为：
其中，为相对位置信息，/>为速度信息，/>为初始化时相机坐标系到IMU坐标系的旋转四元数，ba为加速度、bw为角加速度，w为局部坐标系，bk为k时刻的IMU坐标系，b为IMU坐标系，k为时间；利用所述GNSS定位信息对所述第一状态信息进行优化，得到所述智能农机的第二状态信息；包括：
步骤1：根据所述GNSS定位信息得到所述智能农机的全球位置信息，根据所述全球位置信息和所述第一状态信息中的相对位置信息，得到全球坐标系和局部坐标系的转换偏航角；
步骤2：利用因子图优化算法，通过滑动窗口对所述GNSS定位信息和所述第一状态信息进行实时融合，更新所述第一状态信息中的相对位置信息、速度信息、加速度和角加速度，得到所述智能农机的第二状态信息；包括：
步骤2.1：构建优化问题并建立所述优化问题的约束；
所述优化问题为：
其中，为滑动窗口下的智能农机的状态，/>为/>的最优解，n为滑动窗口的大小，m为图像的特征点个数，ρm为第m个特征点，/>为全球坐标系和局部坐标系的转换偏航角，{rp,Hp}表示系统封装的先验边缘化信息，rp为先验残差，Hp为先验的海森矩阵zi为智能农机中o个独立传感器测量的集合，||·||2为范数运算，r(·)为表示每个独立传感器测量的残差函数，/>为马氏范数；
所述约束包括：
基于所述视觉信息的约束：利用图像的特征点的重投影误差对所述智能农机的状态进行约束；
基于所述惯性导航信息的约束：利用IMU测量的偏差对所述智能农机的状态进行约束；
基于所述GNSS定位信息的约束：利用所述智能农机的全球位置信息与根据所述视觉信息和所述惯性导航信息获取的局部坐标信息之间的残差，对所述智能农机的状态进行约束；
步骤2.2：在建立的约束下，对所述优化问题进行求解，得到所述智能农机的第二状态信息；
其中，在所述基于所述GNSS定位信息的约束中，所述残差表示为：
其中，rp(·)为位置残差，为GNSS接收机在k时刻下的测量值，/>为智能农机的全球位置信息，e为全球坐标系，n为ENU坐标系，/>为局部坐标系到ENU坐标系之间的旋转矩阵，/>为ENU坐标系到全球坐标系的旋转矩阵，/>表示在智能农机正常运行下，将根据GNSS定位信息获取的第一个有效坐标作为锚点的坐标；
其中，若GNSS的定位信号正常，则所述GNSS定位信息为实时获取的GNSS定位信息，若GNSS的定位信号异常，则所述GNSS定位信息为GNSS的定位信号异常前最后一次获取的GNSS定位信息；
其中，在所述基于所述视觉信息的约束中，所述特征点的投影过程建模为：
其中，为图像平面中的特征坐标，u为相机坐标系下x方向的值，v为相机坐标系下y方向的值，T为转置，πc为相机投影函数，c为相机坐标系，/>为IMU坐标系向相机坐标系转换的旋转矩阵，/>为局部坐标系向IMU坐标系转换的旋转矩阵，/>为局部坐标系向IMU坐标系转换的平移向量，/>为IMU坐标系向相机坐标系转换的平移向量，xw为相机在局部坐标系中的位置，nc为测量噪声；
所述特征点的重投影误差表示为：
其中，为两帧图像之间的残差，/>为特征l的测量，/>为特征l在j时刻的相机坐标系的坐标，/>为特征l，/>为局部坐标系向j时刻的IMU坐标系转换的旋转矩阵，/>为i时刻的IMU坐标系向局部坐标系转换的旋转矩阵，/>为相机坐标系向IMU坐标系转换的旋转矩阵，ρl为特征l的深度，/>为相机坐标系向IMU坐标系转换的平移向量，/>为i时刻的IMU坐标系向局部坐标系转换的平移向量，/>为局部坐标系向j时刻的IMU坐标系转换的平移向量；
用户端，用于接收所述智能农机的第二状态信息，并利用所述第二状态信息实现所述智能农机的导航定位。
4.根据权利要求3所述的适用于自动驾驶农机的多传感器融合导航定位系统，其特征在于，
所述信息采集与处理装置，还用于根据所述视觉信息和所述第二状态信息，得到图像的特征点在全球坐标系下的三维坐标；根据所述图像的特征点在全球坐标系下的三维坐标与所述智能农机的第二状态信息，获取所述智能农机与障碍物之间的绝对距离；若所述绝对距离小于预设的安全距离，则生成紧急暂停指令，并将所述紧急暂停指令发送至所述智能农机，以使所述智能农机的自动驾驶设备根据接收的所述紧急暂停指令，控制所述智能农机停止运行。
CN202311402731.4A 2023-10-27 2023-10-27 适用于自动驾驶农机的多传感器融合导航定位系统及方法 Active CN117128951B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311402731.4A CN117128951B (zh) 2023-10-27 2023-10-27 适用于自动驾驶农机的多传感器融合导航定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311402731.4A CN117128951B (zh) 2023-10-27 2023-10-27 适用于自动驾驶农机的多传感器融合导航定位系统及方法

Publications (2)

Publication Number Publication Date
CN117128951A CN117128951A (zh) 2023-11-28
CN117128951B true CN117128951B (zh) 2024-02-02

Family

ID=88851223

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311402731.4A Active CN117128951B (zh) 2023-10-27 2023-10-27 适用于自动驾驶农机的多传感器融合导航定位系统及方法

Country Status (1)

Country Link
CN (1) CN117128951B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110542916A (zh) * 2019-09-18 2019-12-06 上海交通大学 卫星与视觉紧耦合定位方法、系统及介质
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质
CN115435779A (zh) * 2022-08-17 2022-12-06 南京航空航天大学 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
CN116753948A (zh) * 2023-06-16 2023-09-15 中国人民解放军战略支援部队信息工程大学 一种基于视觉惯性gnss ppp耦合的定位方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180112985A1 (en) * 2016-10-26 2018-04-26 The Charles Stark Draper Laboratory, Inc. Vision-Inertial Navigation with Variable Contrast Tracking Residual
US10371530B2 (en) * 2017-01-04 2019-08-06 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry
CN111561923B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 基于多传感器融合的slam制图方法、系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110542916A (zh) * 2019-09-18 2019-12-06 上海交通大学 卫星与视觉紧耦合定位方法、系统及介质
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质
CN113029137A (zh) * 2021-04-01 2021-06-25 清华大学 一种多源信息自适应融合定位方法及系统
CN115435779A (zh) * 2022-08-17 2022-12-06 南京航空航天大学 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
CN116753948A (zh) * 2023-06-16 2023-09-15 中国人民解放军战略支援部队信息工程大学 一种基于视觉惯性gnss ppp耦合的定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Adaptive Sensor Fusion of Camera, GNSS and IMU for Autonomous Driving Navigation;Weining Ren et al.;2020 4th CAA International Conference on Vehicular Control and Intelligence (CVCI);全文 *
ＧＰＳ／ＧＬＯＮＡＳＳ／ＢＤＳ组合的ＲTＫ模型与精度分析;刘金海 等;测绘科学;第44卷(第1期);全文 *
基于时间同步优化和特征筛选的无人机单目视觉 惯导联合定位方法;谭立超;武汉大学学报;第56卷(第2期);全文 *

Also Published As

Publication number Publication date
CN117128951A (zh) 2023-11-28

Similar Documents

Publication Publication Date Title
CN109887057B (zh) 生成高精度地图的方法和装置
KR101220527B1 (ko) 센서 시스템, 이를 이용하는 환경 지도 작성 시스템 및 방법
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN110411457B (zh) 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
US20100164807A1 (en) System and method for estimating state of carrier
CN112476433B (zh) 一种基于识别阵列边界的移动机器人定位方法
CN112562052B (zh) 一种面向近岸水域的实时定位与建图方法
CN113820735B (zh) 位置信息的确定方法、位置测量设备、终端及存储介质
CN110751123B (zh) 一种单目视觉惯性里程计系统及方法
CN111077907A (zh) 一种室外无人机的自主定位方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
CN108613675B (zh) 低成本无人飞行器移动测量方法及系统
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
Li et al. Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system
CN110986888A (zh) 一种航空摄影一体化方法
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN112556681B (zh) 一种基于视觉的果园机械导航定位方法
CN117128951B (zh) 适用于自动驾驶农机的多传感器融合导航定位系统及方法
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
CN115930948A (zh) 一种果园机器人融合定位方法
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant