CN116500638B - 一种基于slam技术的收割机耕道自动导航方法及*** - Google Patents

一种基于slam技术的收割机耕道自动导航方法及*** Download PDF

Info

Publication number
CN116500638B
CN116500638B CN202310750642.2A CN202310750642A CN116500638B CN 116500638 B CN116500638 B CN 116500638B CN 202310750642 A CN202310750642 A CN 202310750642A CN 116500638 B CN116500638 B CN 116500638B
Authority
CN
China
Prior art keywords
point cloud
path
harvester
node
tractor
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
CN202310750642.2A
Other languages
English (en)
Other versions
CN116500638A (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.)
Jiangsu University
Original Assignee
Jiangsu University
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 Jiangsu University filed Critical Jiangsu University
Priority to CN202310750642.2A priority Critical patent/CN116500638B/zh
Publication of CN116500638A publication Critical patent/CN116500638A/zh
Application granted granted Critical
Publication of CN116500638B publication Critical patent/CN116500638B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • 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/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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种基于SLAM技术的收割机耕道自动导航方法及***，针对机耕道场景，采用激光雷达与GPS结合增加机耕道点云处理算法的NDT算法构建出机耕道场景下的高精度点云地图，利用A*全局路径规划算法,规划机耕道下的最优行驶路径，并通过Pure_pursuit算法对规划路径进行跟踪，使收割机不偏离规划路径，通过CAN总线向底层液压控制器驱动液压阀驱动收割机往前行驶，最终实现了机耕道下的自动导航。本发明适合收割场景下的机耕道路的行驶，面对一些GPS信号比较弱的农田，利用激光雷达的SLAM技术，解决了GPS不稳定情况下的自动导航问题。另外，通过机耕道点云处理算法消除点云噪声，提高地图构建精度，满足机耕道的自动导航需求。

Description

一种基于SLAM技术的收割机耕道自动导航方法及***
技术领域
本发明属于自动导航技术领域，尤其涉及一种基于SLAM技术的收割机耕道自动导航方法及***。
背景技术
近年来，随着无人驾驶技术的不断发展，收割机的无人化程度不断提高。实现收割机自动驾驶功能可以节省劳动力，提高工作效率。
目前而言，收割机的自动导航都是单纯的依靠GPS实现的。这种导航方式的缺点是在GPS信号较弱或没有GPS信号时，无法实现自身的定位，无法进行路径规划，进而无法实现自动导航。如在机耕道路上，两侧的树木较多，GPS信号容易受到树木的干扰，另外收割机附近GPS基站信号的强弱，也会影响GPS信号的正常接收。若GPS信号受到影响，收割机在机耕道下的自动导航功能则无法正常实现。
SLAM又称同时建图和定位技术，通过该方法可构建点云地图同时实现定位功能，为自动导航服务。利用SLAM技术结合路径决策控制算法的导航方法相比与传统的GPS导航，有定位精度高和场景信息丰富的优点，不会受到机耕道两侧树木或者基站信号强度的影响。
发明内容
针对上述技术问题，本发明的一个方式的目的之一是提供一种基于SLAM技术的收割机耕道自动导航方法，利用多线激光雷达获取收割机耕道场境下的三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，并通过对机耕道路上点云处理和添加GPS约束，实现对机耕道场景的高精度点云地图的构建；地图构建完成后通过A*路径规划算法规划收割机的最优机耕道路径，并使用Pure_pursuit路径跟踪算法对规划路径进行跟踪，实现收割机在机耕道场景下的自动导航。本方法能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
本发明的一个方式的目的之一是提供一种基于SLAM技术的收割机耕道自动导航***，包括感知模块、决策模块和执行模块；所述感知模块包括多线激光雷达和GPS天线，用于获取收割机周围的环境信息以及收割机的位姿状态；所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
注意，这些目的的记载并不妨碍其他目的的存在。本发明的一个方式并不需要实现所有上述目的。可以从说明书、附图、权利要求书的记载中抽取上述目的以外的目的。
本发明是通过以下技术手段实现上述技术目的的。
一种基于SLAM技术的收割机耕道自动导航方法，包括以下步骤：
步骤S1、***初始化：对***进行配置和初始化；
步骤S2、环境感知：获取多线激光雷达获取点云信息以及GPS的位姿信息；
步骤S3、地图构建：根据步骤S2获取的环境三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；
所述机耕道点云处理算法具体包括以下步骤：
首先对机耕道上的噪声点云进行点云滤除，设t时刻的点云为pij,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为Lg，动态目标的点云标签为Ld，再对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组Pg，如果属于动态目标类的点云，则移动到动态目标点云Pd，通过以上的遍历，将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通过对机耕道上滤除后的点云的邻域进行插值，生成新的点，增加点云数据的密度，得到更清晰的机耕道的特征；
随后设置输入点云组和参考点云组，然后对输入点云组运动量做平移和旋转变换得到变换后的点云组P′，将变换后的点云组P′转换到参考点云组Q，根据下面公式得到点云的概率f(k)：
其中，p′ k为P′中的第k个点云，μa,b为参考点云组中Cell(a,b)单元格的平均值，Σa,b为参考点云组中Cell(a,b)单元格的协方差；
将输入点云组中的n个点的概率求和，求其平均值作为配准分数，其计算公式如下：
其中，d是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其满足迭代条件，并使得分最大，得到三维点云地图；
步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，完成全局地图构建；
步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；
步骤S6、路径跟踪：按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪。
上述方案中，所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
上述方案中，所述步骤S4地图约束具体包括以下步骤：
在三维点云地图中加入GPS信息，首先判断第i帧特征点云是否为关键帧，并且定位误差ei大于阈值emax,则根据时间戳在GPS列表中查找最近的点，并在NDT算法中添加GPS约束，通过目标优化函数优化地图。
进一步的，所述目标优化函数J公式如下：
其中，ε为所有边的集合，i,j为任意两个顶点的下标，eij为i,j两点的误差，为eij的转置，/>为GPS观测的协方差矩阵，表示为：
其中，Δt为GPS节点与其相邻最近的节点时间戳的差值。
上述方案中，所述步骤S5路径规划包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合Open list和Close list,其中Open list集合存放可能为最短路径的节点，Closelist集合存放已找出的最短路径的节点；
步骤S5.2：将起点放入Open list中，遍历所有与起点相邻的节点，将符合条件的节点放入Open list中，将起点设置为父节点，根据以下公式得到节点的路径长度：
f(n)＝g(n)+h(n)，
其中，f(n)为节点的评估函数，g(n)为起点到节点n的路径代价，h(n)代表节点n到终点的估计路径代价；再删除在Open list中的节点，并将起点加入到Close list中；
步骤S5.3:如果Open list中没有节点，则执行步骤S5.6，否则，从Open list中找到路径最小节点x,并删除Open list中的节点，并将该节点加入到Close list中；
步骤S5.4：遍历搜索与节点x相邻的节点，计算这些节点的路径长度，并将节点x设置为父节点；搜索Open list中与节点x相邻节点，若存在节点并且以x为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在Open list中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
上述方案中，所述步骤S6路径跟踪Pure_pursuit算法的控制效果取决于预瞄距离ld，ld具体的公式如下：
ld＝Av2+Bv+C，
其中，amax为收割机的最大制动减速度，Av2表示收割机的最短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最小转弯半径。
一种实现上述基于SLAM技术的收割机耕道自动导航方法的***，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，多线激光雷达用于获取收割机周围的环境信息，GPS天线用于获取收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
上述方案中，所述决策模块的边缘计算平台采用Linux***的嵌入式开发板，嵌入式开发板安装在收割机驾驶室内，通过CAN总线与收割机底层液压控制器连接，通过网口与激光雷达连接，通过串口与GPS天线连接。
上述方案中，所述执行模块通过收割机底层液压控制器控制液压***电磁阀，驱动收割机行驶。
与现有技术相比，本发明的有益效果是：
本发明，利用多线激光雷达获取收割机耕道场境下的三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，并通过对机耕道点云处理算法和添加GPS约束，实现对机耕道场景的高精度点云地图的构建；地图构建完成后通过A*路径规划算法规划收割机的最优机耕道路径，并使用Pure_pursuit路径跟踪算法对规划路径进行跟踪，实现收割机在机耕道场景下的自动导航。本方法能够有效解决收割机在弱GPS信号的情况下的自动驾驶问题。
根据本发明的一个方式，通过在点云配准算法NDT中加入机耕道点云处理算法，可以构建高精度点云地图。
根据本发明的一个方式，构建全局地图时，GPS仅对点云地图进行约束，而不是全部依靠GPS信息构建地图，可以减少对GPS的依赖，同时调高全局地图精度。
根据本发明的一个方式，在进行路径规划之后，采用Pure_pursuit算法对规划的路径进行跟踪，可以使得收割机不偏离规划的路径，更加精准的完成作业任务。
注意，这些效果的记载不妨碍其他效果的存在。本发明的一个方式并不一定必须具有所有上述效果。可以从说明书、附图、权利要求书等的记载显而易见地看出并抽出上述以外的效果。
附图说明
图1是本发明一实施方式的自动导航***结构示意图。
图2是本发明一实施方式的自动导航***的硬件安装位置示意图。
图3是本发明一实施方式的地图构建流程示意图。
图4是本发明一实施方式的地图构建过程示意图。
图5是本发明一实施方式的地图构建效果示意图。
图6(a)是没有机耕道点云处理算法的NDT地图构建效果示意图，图6(b)是增加机耕道点云处理算法的NDT地图构建效果示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“前”、“后”、“左”、“右”、“上”、“下”、“轴向”、“径向”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
实施例1
图1所示为所述基于SLAM技术的收割机耕道场自动导航***的一种较佳实施方式，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，用于获取收割机周围的环境信息以及收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
如图2所示，根据本实施例，优选的，激光雷达采用多线激光雷达，多线激光雷达被安装在收割机驾驶室的定位位置，安装的倾斜角度为30°，可以探测收割机前方150米的三维点云信息，能够满足实际需要。
根据本实施例，优选的，GPS天线被安装收割机驾驶室顶部后方位置，可稳定的接收GPS信号。决策模块采用嵌入式开发板是Jetson Agx Xavier,它具有尺寸小，计算性能高，可进行CUDA加速等优点，可满足***要求。该嵌入式开发板被安装在收割机的驾驶室内，使用收割机的电源进行供电，通过CAN总线与收割机底层液压控制器连接，通过网口与激光雷达连接，通过串口与GPS天线连接。收割机底层液压控制器接收到CAN总线发送的指令后通过电磁阀驱动液压阀使得收割机开始进行动作。
实施例2
一种基于SLAM技术的收割机耕道自动导航方法，包括以下步骤：
步骤S1、***初始化：对***进行配置和初始化；根据本实施例，优选的，多线激光雷达被安装在收割机驾驶舱顶部的前方，GPS天线被安装在收割机驾驶舱顶部的后方位置，接收机安装在驾驶舱内，嵌入式开发板被安装在驾驶舱内，相应的连接线缆也被安装在驾驶舱内。安装完成后，对激光雷达和GPS天线进行软件配置，使其可以正常发送数据；
步骤S2、环境感知：获取周围的环境信息以及收割机的位姿状态；
步骤S3、地图构建：根据步骤S2获取的环境信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；
步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，减少累计误差，完成全局地图构建；
步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；
步骤S6、路径跟踪：采用执行模块，按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪。
所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
所述步骤S3在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图具体包括以下步骤：
步骤S3.1：为了减少机耕道地面的噪声点云的干扰，特别地使用噪声点云滤除算法，去除掉噪声点云，提供场景建图的效果。具体地如下：设t时刻的点云为pij,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为Lg，动态目标的点云标签为Ld；其次，对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组Pg，如果属于动态目标类的点云，则移动到动态目标点云Pd，通过以上的遍历，可将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通对机耕道上滤除后的点云的邻域进行插值，生成新的点，以增加点云数据的密度，得到更清晰的机耕道的特征；
步骤S3.2：首先，设有一组参考点云组Q和输入点云组P，将参考点云组划分为m×n个单元格，对于某个单元格为Cell(m,n)，μm,n为参考点云组中Cell(m,n)单元格的平均值，Σm,n为参考点云组中Cell(m,n)单元格的协方差；
步骤S3.3：对输入点云组的运动量d做平移变换t和旋转变换R得到变换后的输入点云组P′,其中p′ k为P′中的第k个点云，μa,b为参考点云组中Cell(a,b)单元格的平均值，Σa,b为参考点云组中Cell(a,b)单元格的协方差；
步骤S3.4：将变换后的输入点云组P′转换到参考点云组Q中，可以得到点云的概率f(k):
步骤S3.5：将输入点云组中n个点的概率进行求和，并将平均值作为配准分数：
其中，d是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其可满足迭代条件，并使得分最大，以便更加精准的构建点云地图，地图构建过程效果图如图4所示。
图6(a)是没有机耕道点云处理算法的NDT算法构建的地图构建效果示意图，图6(b)是增加机耕道点云处理算法的NDT算法建图效果示意图，在经过机耕道点云处理算法之后构建的点云地图更加精准，噪声更小，地图更为符合实际工作场景。本发明地图构建的方法与普通算法相比，可以满足在机耕道场景下的地图构建，适合收割机自动导航的要求。
所述步骤S4地图约束具体包括以下步骤：
通过加入GPS信息对构建的机耕道场景地图进行约束，较少算法的累计误差。具体地，采集点云数据和GPS数据时，利用文本文档保存第一帧点云数据对应得到GPS信息中的经纬度和高度信息，并转换到UTM坐标下。加入GPS约束之后，GPS与位姿节点之间的约束强弱取决于该位姿对应的关键帧时间戳和采集GPS信号的时间间隔，时间间隔越短，GPS对位姿节点的约束越强。
GPS约束添加流程主要如下：首先判断第i帧特征点云是否为关键帧，并且定位误差ei大于阈值emax,则根据时间戳在GPS列表中查找最近的点，并在NDT算法中添加GPS约束，通过目标优化函数，减少累积误差，提高三维点云地图精确度。
目标优化函数J表示为：
其中，ε为所有边的集合，i,j为任意两个顶点的下标，eij为i,j两点的误差，为eij的转置，/>为GPS观测的协方差矩阵，可表示为：
其中，Δt为GPS节点与其相邻最近的节点时间戳的差值，Δt越大，GPS观测对里程计位姿观测的误差越大，反之，误差越小。
在完成GPS约束之后，全局地图的最终构建效果如图5所示。
所述步骤S5路径规划采用使用A*算法，具体包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合Open list和Close list,其中Open list集合存放可能为最短路径的节点，Closelist集合存放已找出的最短路径的节点；
步骤S5.2：将起点放入Open list中，遍历所有与起点相邻的节点，将符合条件的节点放入Open list中，将起点设置为父节点，根据以下公式可得到节点的路径长度：
f(n)＝g(n)+h(n)，
其中，f(n)为节点的评估函数，g(n)为起点到节点n的路径代价，h(n)代表节点n到终点的估计路径代价；再删除在Open list中的节点，并将起点加入到Close list中；
步骤S5.3:如果Open list中没有节点，则执行步骤S5.6，否则，从Open list中找到路径最小节点x,并删除Open list中的节点，并将该节点加入到Close list中；
步骤S5.4：遍历搜索与节点x相邻的节点，计算这些节点的路径长度，并将节点x设置为父节点；搜索Open list中与节点x相邻节点，若存在节点并且以x为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在Open list中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
所述步骤S6路径跟踪Pure_pursuit算法的控制效果取决于预瞄距离ld，如果ld越大，控制效果会越平滑，ld越小，控制的效果越精确。而ld与收割机当前的速度有关。其中ld具体的求取公式如下：
ld＝Av2+Bv+C，
其中，其中amax为收割机的最大制动减速度，Av2表示收割机的最短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最小转弯半径。
通过上面的公式确定收割机的预瞄距离，可确定Pure_pursuit控制参数，使得该算法可以对收割机的行驶路径进行跟踪控制，最终让收割机可以按照规划的路径行驶。
所述路径行驶具体为在决策模块完成计算后会通过CAN分析仪向底层液压控制器发送CAN指令，收割机的底层液压控制器接收到CAN指令后通过电磁阀驱动收割机的液压阀从而驱动收割机的行走，实现自动行走功能。
本发明基于SLAM技术的收割机耕道场景地图构建方法,针对收割机作业场景下的机耕道场景，采用激光雷达与GPS融合结合NDT点云配准算法构建出机耕道场景下的高精度点云地图，利用A*全局路径规划算法,规划了收割机耕道下的最优行驶路径，并通过Pure_pursuit算法对规划的路径进行跟踪，使得收割机不偏离规划的路径。通过CAN总线向收割机底层液压控制器发送CAN指令，通过电磁阀驱动液压阀驱动收割机往前行驶，最终实现了在机耕道下的自动导航。
本发明适合收割机收割场景下的机耕道路的行驶，面对一些GPS信号比较弱的农田，利用激光雷达的SLAM技术，解决了GPS不稳定情况下的自动导航问题。另外，通过在点云地图构建的时候使用机耕道点云处理算法，使得本发明可以适用于机耕道场景下复杂路面情况造成的点云噪声过多的情形，从而构建满足机耕道场景的地图，实现收割机收割场景下的自动导航。
应当理解，虽然本说明书是按照各个实施例描述的，但并非每个实施例仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施例的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于SLAM技术的收割机机耕道自动导航方法，其特征在于，包括以下步骤：
步骤S1、***初始化：对***进行配置和初始化；
步骤S2、环境感知：获取多线激光雷达获取点云信息以及GPS的位姿信息；
步骤S3、地图构建：根据步骤S2获取的环境三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；
所述机耕道点云处理算法具体包括以下步骤：
首先对机耕道上的噪声点云进行点云滤除，设t时刻的点云为pij,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为Lg，动态目标的点云标签为Ld，再对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组Pg，如果属于动态目标类的点云，则移动到动态目标点云Pd，通过以上的遍历，将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通过对机耕道上滤除后的点云的邻域进行插值，生成新的点，增加点云数据的密度，得到更清晰的机耕道的特征；
随后设置输入点云组和参考点云组，然后对输入点云组运动量做平移和旋转变换得到变换后的点云组P′，将变换后的点云组P′转换到参考点云组Q，根据下面公式得到点云的概率f(k)：
其中，p′k为P′中的第k个点云，μa,b为参考点云组中Cell(a,b)单元格的平均值，Σa,b为参考点云组中Cell(a,b)单元格的协方差；
将输入点云组中的n个点的概率求和，求其平均值作为配准分数，其计算公式如下：
其中，d是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其满足迭代条件，并使得分最大，得到三维点云地图；
步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，完成全局地图构建；
步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；
步骤S6、路径跟踪：按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪。
2.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
3.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S4地图约束具体包括以下步骤：
在三维点云地图中加入GPS信息，首先判断第i帧特征点云是否为关键帧，并且定位误差ei大于阈值emax,则根据时间戳在GPS列表中查找最近的点，并在NDT算法中添加GPS约束，通过目标优化函数优化地图。
4.根据权利要求3所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述目标优化函数J公式如下：
其中，ε为所有边的集合，i,j为任意两个顶点的下标，eij为i,j两点的误差，为eij的转置，/>为GPS观测的协方差矩阵，表示为：
其中，Δt为GPS节点与其相邻最近的节点时间戳的差值。
5.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S5路径规划包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合Open list和Close list,其中Open list集合存放可能为最短路径的节点，Closelist集合存放已找出的最短路径的节点；
步骤S5.2：将起点放入Open list中，遍历所有与起点相邻的节点，将符合条件的节点放入Open list中，将起点设置为父节点，根据以下公式得到节点的路径长度：
f(n)＝g(n)+h(n)，
其中，f(n)为节点的评估函数，g(n)为起点到节点n的路径代价，h(n)代表节点n到终点的估计路径代价；再删除在Open list中的节点，并将起点加入到Close list中；
步骤S5.3:如果Open list中没有节点，则执行步骤S5.6，否则，从Open list中找到路径最小节点x,并删除Open list中的节点，并将该节点加入到Close list中；
步骤S5.4：遍历搜索与节点x相邻的节点，计算这些节点的路径长度，并将节点x设置为父节点；搜索Open list中与节点x相邻节点，若存在节点并且以x为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在Open list中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
6.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S6路径跟踪Pure_pursuit算法的控制效果取决于预瞄距离ld，ld具体的公式如下：
ld＝Av2+Bv+C，
其中，amax为收割机的最大制动减速度，Av2表示收割机的最短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最小转弯半径。
7.一种基于SLAM技术的收割机机耕道自动导航***，用于实现权利要求1-6任意一项所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，多线激光雷达用于获取收割机周围的环境三维点云信息，GPS天线用于获取收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
8.根据权利要求7所述的基于SLAM技术的收割机机耕道自动导航***，其特征在于，所述决策模块的边缘计算平台采用Linux***的嵌入式开发板，嵌入式开发板安装在收割机驾驶室内，通过CAN总线与收割机底层液压控制器连接，通过网口与激光雷达连接，通过串口与GPS天线连接。
9.根据权利要求7所述的基于SLAM技术的收割机机耕道自动导航***，其特征在于，所述执行模块通过收割机底层液压控制器控制液压***电磁阀，驱动收割机行驶。
CN202310750642.2A 2023-06-25 2023-06-25 一种基于slam技术的收割机耕道自动导航方法及*** Active CN116500638B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310750642.2A CN116500638B (zh) 2023-06-25 2023-06-25 一种基于slam技术的收割机耕道自动导航方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310750642.2A CN116500638B (zh) 2023-06-25 2023-06-25 一种基于slam技术的收割机耕道自动导航方法及***

Publications (2)

Publication Number Publication Date
CN116500638A CN116500638A (zh) 2023-07-28
CN116500638B true CN116500638B (zh) 2023-10-10

Family

ID=87318693

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310750642.2A Active CN116500638B (zh) 2023-06-25 2023-06-25 一种基于slam技术的收割机耕道自动导航方法及***

Country Status (1)

Country Link
CN (1) CN116500638B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767860A (zh) * 2016-11-21 2017-05-31 江苏大学 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN113126115A (zh) * 2021-04-06 2021-07-16 北京航空航天大学杭州创新研究院 基于点云的语义slam方法、装置、电子设备和存储介质
CN113740875A (zh) * 2021-08-03 2021-12-03 上海大学 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法
CN113850915A (zh) * 2021-09-09 2021-12-28 桂林电子科技大学 一种基于Autoware的车辆循迹方法
CN113903011A (zh) * 2021-10-26 2022-01-07 江苏大学 一种适用于室内停车场的语义地图构建及定位方法
CN114279434A (zh) * 2021-12-27 2022-04-05 驭势科技（北京）有限公司 一种建图方法、装置、电子设备和存储介质
CN114371716A (zh) * 2022-01-20 2022-04-19 红骐科技(杭州)有限公司 一种用于消防机器人的自动驾驶巡检方法
CN114820505A (zh) * 2022-04-22 2022-07-29 浙江大学 一种动态目标的非接触测量方法
WO2022257801A1 (zh) * 2021-06-09 2022-12-15 山东大学 基于slam的移动机器人矿山场景重建方法及***
CN115855062A (zh) * 2022-12-07 2023-03-28 重庆理工大学 一种室内移动机器人自主建图与路径规划方法
CN116091951A (zh) * 2023-04-07 2023-05-09 华南农业大学 一种农田与机耕道边界线提取方法及***

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767860A (zh) * 2016-11-21 2017-05-31 江苏大学 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN113126115A (zh) * 2021-04-06 2021-07-16 北京航空航天大学杭州创新研究院 基于点云的语义slam方法、装置、电子设备和存储介质
WO2022257801A1 (zh) * 2021-06-09 2022-12-15 山东大学 基于slam的移动机器人矿山场景重建方法及***
CN113740875A (zh) * 2021-08-03 2021-12-03 上海大学 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法
CN113850915A (zh) * 2021-09-09 2021-12-28 桂林电子科技大学 一种基于Autoware的车辆循迹方法
CN113903011A (zh) * 2021-10-26 2022-01-07 江苏大学 一种适用于室内停车场的语义地图构建及定位方法
CN114279434A (zh) * 2021-12-27 2022-04-05 驭势科技（北京）有限公司 一种建图方法、装置、电子设备和存储介质
CN114371716A (zh) * 2022-01-20 2022-04-19 红骐科技(杭州)有限公司 一种用于消防机器人的自动驾驶巡检方法
CN114820505A (zh) * 2022-04-22 2022-07-29 浙江大学 一种动态目标的非接触测量方法
CN115855062A (zh) * 2022-12-07 2023-03-28 重庆理工大学 一种室内移动机器人自主建图与路径规划方法
CN116091951A (zh) * 2023-04-07 2023-05-09 华南农业大学 一种农田与机耕道边界线提取方法及***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
无人驾驶农业机械导航***研究;朱世威;《中国优秀硕士学位论文全文数据库农业科技辑》(第03期);正文全文 *
智能收获机定位和自适应路径追踪方法;王立辉 等;《导航定位学报》;第8卷(第6期);29-36 *

Also Published As

Publication number Publication date
CN116500638A (zh) 2023-07-28

Similar Documents

Publication Publication Date Title
CN111949943B (zh) 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
JP6928722B2 (ja) 車両動作のための位置特定の決定
CN103926925B (zh) 一种基于改进的vfh算法的定位与避障方法及机器人
CN114812581B (zh) 一种基于多传感器融合的越野环境导航方法
CN107015238A (zh) 基于三维激光雷达的无人车自主定位方法
Pfaff et al. Towards mapping of cities
CN110187372B (zh) 一种低速无人车园区内组合导航方法及***
US11834077B2 (en) Systems, methods, and media for occlusion-aware motion planning
CN112859860A (zh) 一种机器人***及其路径规划方法
CN114295132A (zh) 使用递归自组织地图的虚拟行道估测
CN115371662B (zh) 一种基于概率栅格移除动态对象的静态地图构建方法
CN112230638A (zh) 一种车辆的停车路径规划方法和装置
CN115127572A (zh) 一种露天矿无人驾驶道路的路径生成和更新方法
CN115145315A (zh) 一种改进a*算法的适合杂乱环境的无人机路径规划方法
WO2022178738A1 (en) Method and system for generating a topological graph map
CN116500638B (zh) 一种基于slam技术的收割机耕道自动导航方法及***
WO2022068033A1 (en) Method and system for real-time landmark extraction from a sparse three-dimensional point cloud
CN116929363A (zh) 一种基于可通行地图的矿用车辆自主导航方法
US11783178B2 (en) Systems and methods for corridor intent prediction
CN113183943B (zh) 一种农用装备的智能驾驶***及其作业方法
US20220196828A1 (en) Radar Reference Map Generation
Xiang et al. Where am I? SLAM for mobile machines on a smart working site
CN114543793A (zh) 车辆导航***的多传感器融合定位方法
Lee et al. Passage Planning in Coastal Waters for Maritime Autonomous Surface Ships using the D* Algorithm
CN116222544B (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