CN116500638A - 一种基于slam技术的收割机机耕道自动导航方法及系统 - Google Patents
一种基于slam技术的收割机机耕道自动导航方法及系统 Download PDFInfo
- Publication number
- CN116500638A CN116500638A CN202310750642.2A CN202310750642A CN116500638A CN 116500638 A CN116500638 A CN 116500638A CN 202310750642 A CN202310750642 A CN 202310750642A CN 116500638 A CN116500638 A CN 116500638A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- path
- harvester
- tractor
- ploughing
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000005516 engineering process Methods 0.000 title claims abstract description 26
- 238000003971 tillage Methods 0.000 title abstract description 14
- 238000010276 construction Methods 0.000 claims abstract description 29
- 238000012545 processing Methods 0.000 claims abstract description 29
- 230000000694 effects Effects 0.000 claims description 17
- 238000005457 optimization Methods 0.000 claims description 9
- 238000011161 development Methods 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- 238000012217 deletion Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000004044 response Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 2
- 238000003306 harvesting Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 9
- 238000009434 installation Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- HPTJABJPZMULFH-UHFFFAOYSA-N 12-[(Cyclohexylcarbamoyl)amino]dodecanoic acid Chemical compound OC(=O)CCCCCCCCCCCNC(=O)NC1CCCCC1 HPTJABJPZMULFH-UHFFFAOYSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001151 other effect Effects 0.000 description 1
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技术的收割机机耕道自动导航方法及系统。
背景技术
近年来，随着无人驾驶技术的不断发展，收割机的无人化程度不断提高。实现收割机自动驾驶功能可以节省劳动力，提供工作效率。
目前而言，收割机的自动导航都是单纯的依靠GPS实现的。这种导航方式的缺点是在GPS信号较弱或没有GPS信号时，无法实现自身的定位，无法进行路径规划，进而无法实现自动导航。如在机耕道路上，两侧的树木较多，GPS信号容易受到树木的干扰，另外收割机附近GPS基站信号的强弱，也会影响GPS信号的正常接收。若GPS信号受到影响，收割机在机耕道下的自动导航功能则无法正常实现。
SLAM又称同时建图和定位技术，通过该方法可构建点云地图同时实现定位功能，为自动导航服务。利用SLAM技术结合路径决策控制算法的导航方法相比与传统的GPS导航，有定位精度高和场景信息丰富的优点，不会受到机耕道两侧树木或者基站信号强度的影响。
发明内容
针对上述技术问题，本发明的一个方式的目的之一是提供一种基于SLAM技术的收割机机耕道自动导航方法，利用多线激光雷达获取收割机机耕道场境下的三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，并通过对机耕道路上点云处理和添加GPS约束，实现对机耕道场景的高精度点云地图的构建；地图构建完成后通过A*路径规划算法规划收割机的最优机耕道路径，并使用路径跟踪算法对规划路径进行跟踪，实现收割机在机耕道场景下的自动导航。本方法能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
本发明的一个方式的目的之一是提供一种基于SLAM技术的收割机机耕道自动导航系统，包括感知模块、决策模块和执行模块；所述感知模块包括多线激光雷达和GPS天线，用于获取收割机周围的环境信息以及收割机的位姿状态；所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
注意，这些目的的记载并不妨碍其他目的的存在。本发明的一个方式并不需要实现所有上述目的。可以从说明书、附图、权利要求书的记载中抽取上述目的以外的目的。
本发明是通过以下技术手段实现上述技术目的的。
一种基于SLAM技术的收割机机耕道自动导航方法，包括以下步骤：
步骤S1、系统初始化：对系统进行配置和初始化；
步骤S2、环境感知：获取多线激光雷达获取点云信息以及GPS的位姿信息；
步骤S3、地图构建：根据步骤S2获取的环境三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；
步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，完成全局地图构建；
步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；
步骤S6、路径跟踪：按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_ pursuit对规划好的路径进行跟踪。
上述方案中，所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
上述方案中，所述步骤S3在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图具体包括以下步骤：
首先对机耕道上的噪声点云进行点云滤除，设t时刻的点云为,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为/>，动态目标的点云标签为/>，再对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组/>，如果属于动态目标类的点云，则移动到动态目标点云/>，通过以上的遍历，将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通过对机耕道上滤除后的点云的邻域进行插值，生成新的点，增加点云数据的密度，得到更清晰的机耕道的特征；
随后设置输入点云组和参考点云组，然后对输入点云组运动量做平移和旋转变换得到变换后的点云组，将变换后的点云组/>转换到参考点云组Q，根据下面公式得到点云的概率/>：
，
其中，为/>中的某个单元格，/>为参考点云组中/>单元格的平均值，/>为参考点云组中/>单元格的协方差；
将输入点云组中的n个点的概率求和，求其平均值作为配准分数，其计算公式如下：
，
其中，是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其满足迭代条件，并使得分最大，得到三维点云地图。
上述方案中，所述步骤S4地图约束具体包括以下步骤：
在三维点云地图中加入GPS信息，首先判断第i帧特征点云是否为关键帧，并且定位误差大于阈值/>,则根据时间戳在GPS列表中查找最近的点在NDT算法中添加GPS约束，通过目标优化函数优化地图。
进一步的，所述目标优化函数公式如下：
，
其中，为所有边的集合，/>为任意两个顶点的下标，/>为/>两点的误差，/>为/>的转置，/>为GPS观测的协方差矩阵，表示为：
，
其中，为GPS节点与其相邻最近的节点时间戳的差值。
上述方案中，所述步骤S5路径规划包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合和/>,其中/>集合存放可能为最短路径的节点，/>已找出的最短路径的节点集合；
步骤S5.2：将起点放入中，遍历所有与起点相邻的节点，将符合条件的节点放入/>中，将起点设置为父节点，根据以下公式得到节点的路径长度：
，
其中，为节点的评估函数，/>为起点到节点n的路径代价，/>代表节点n到终点的估计路径代价；再删除在/>中的节点，并将起点加入到/>中；
步骤S5.3:如果中没有节点，则执行步骤S5.6，否则，从/>中找到路径最小节点/>,并删除/>中的节点，并将该节点加入到/>中；
步骤S5.4：遍历搜索与节点相邻的节点，计算这些节点的路径长度，并将节点/>设置为父节点；搜索/>中与节点/>相邻节点，若存在节点并且以/>为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
上述方案中，所述步骤S6路径跟踪算法的控制效果取决于预瞄距离，/>具体的公式如下：
，
其中，,为收割机的最大制动减速度，表示收割机的最
短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最
小转弯半径。
一种实现上述基于SLAM技术的收割机机耕道自动导航方法的系统，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，多线激光雷达用于获取收割机周围的环境三维点云信息，GPS天线用于获取收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
上述方案中，所述决策模块的边缘计算平台采用Linux系统的嵌入式开发板，嵌入式开发板安装在收割机驾驶室内，通过CAN总线与收割机底层液压控制器连接，通过网口与激光雷达连接，通过串口与GPS天线连接。
上述方案中，所述执行模块通过收割机底层液压控制器控制液压系统电磁阀，驱动收割机行驶。
与现有技术相比，本发明的有益效果是：
本发明，利用多线激光雷达获取收割机机耕道场境下的三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，并通过对机耕道点云处理算法和添加GPS约束，实现对机耕道场景的高精度点云地图的构建；地图构建完成后通过A*路径规划算法规划收割机的最优机耕道路径，并使用路径跟踪算法对规划路径进行跟踪，实现收割机在机耕道场景下的自动导航。本方法能够有效解决收割机在弱GPS信号的情况下的自动驾驶问题。
根据本发明的一个方式，通过在点云配准算法NDT中加入机耕道点云处理算法，可以构建高精度点云地图。
根据本发明的一个方式，构建全局地图时，GPS仅对点云地图进行约束，而不是全部依靠GPS信息构建地图，可以减少对GPS的依赖，同时调高全局地图精度。
根据本发明的一个方式，在进行路径规划之后，采用算法对规划的路径进行跟踪，可以使得收割机不偏离规划得路径，更加精准的完成作业任务。
注意，这些效果的记载不妨碍其他效果的存在。本发明的一个方式并不一定必须具有所有上述效果。可以从说明书、附图、权利要求书等的记载显而易见地看出并抽出上述以外的效果。
附图说明
图1是本发明一实施方式的自动导航系统结构示意图。
图2是本发明一实施方式的自动导航系统的硬件安装位置示意图。
图3是本发明一实施方式的地图构建流程示意图。
图4是本发明一实施方式的地图构建过程示意图。
图5是本发明一实施方式的地图构建效果示意图。
图6是本发明点云地图构建效果对比图，其中图6（a）是没有机耕道点云处理算法的NDT地图构建效果示意图，图6（b）是增加机耕道点云处理算法的NDT地图构建效果示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“前”、“后”、“左”、“右”、“上”、“下”、“轴向”、“径向”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
实施例1
图1所示为所述于SLAM技术的收割机机耕道场自动导航系统的一种较佳实施方式，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，用于获取收割机周围的环境信息以及收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
如图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时刻的点云为,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为/>，动态目标的点云标签为；其次，对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组/>，如果属于动态目标类的点云，则移动到动态目标点云/>，通过以上的遍历，可将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通对机耕道上滤除后的点云的邻域进行插值，生成新的点，以增加点云数据的密度，得到更清晰的机耕道的特征；
步骤S3.2：首先，设有一组参考点云组Q和输入点云组P，将参考点云组划分为m×n
个单元格，对于某个单元格为，为参考点云组中单元格的平均值，为参考点云组中单元
格的协方差；
步骤S3.3：对输入点云组的运动量d做平移变换t和旋转变换R得到变换后的输入点云组,其中/>为/>中的某个单元格，/>为参考点云组中/>单元格的平均值，/>为参考点云组中/>单元格的协方差
步骤S3.4：将变换后的点云组转换到参考点云组Q，根据下面公式得到点云的概率/>:
，
步骤S3.5：将输入点云组中n个点的概率进行求和，并将平均值作为配准分数：
，
其中，d是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其可满足迭代条件，并使得分最大，以便更加精准的构建点云地图，地图构建过程效果图如图4所示。
根据图6所示，图6（a）是没有机耕道点云处理算法的NDT算法构建的地图构建效果示意图，图6（b）是增加机耕道点云处理算法的NDT算法建图效果示意图，在经过机耕道点云处理算法之后构建的点云地图更加精准，噪声更小，地图更为符合实际工作场景。本发明地图构建的方法与普通算法相比，可以满足在机耕道场景下的地图构建，适合收割机自动导航的要求。
所述步骤S4地图约束具体包括以下步骤：
通过加入GPS信息对构建的机耕道场景地图进行约束，较少算法的累计误差。具体地，采集点云数据和GPS数据时，利用文本文档保存第一帧点云数据对应得到GPS信息中的经纬度和高度信息，并转换到UTM坐标下。加入GPS约束之后，GPS与位姿节点之间的约束强弱取决于该位姿对应的关键帧时间戳和采集GPS信号的时间间隔，时间间隔越短，GPS对位姿节点的约束越强。
GPS约束添加流程主要如下：首先判断第i帧特征点云是否为关键帧，并且定位误
差大于阈值,则根据时间戳在GPS列表中查找最近的点在NDT算法中添加GPS约束，通
过目标优化函数，减少累积误差，提高三维点云地图精确度。
目标优化函数表示为：
，
其中，为所有边的集合，/>为任意两个顶点的下标，/>为/>两点的误差，/>为/>的转置，/>为GPS观测的协方差矩阵，可表示为：
，
其中，为GPS节点与其相邻最近的节点时间戳的差值，越大，GPS观测对里程计位姿观测的误差越大，反之，误差越小。
在完成GPS约束之后，全局地图的最终构建效果如图5所示。
所述步骤S5路径规划采用使用A*算法，具体包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合和/>,其中/>集合存放可能为最短路径的节点，/>已找出的最短路径的节点集合；
步骤S5.2：将起点放入中，遍历所有与起点相邻的节点，将符合条件的节点放入/>中，将起点设置为父节点，根据以下公式得到节点的路径长度：
，
其中，为节点的评估函数，/>为起点到节点n的路径代价，/>代表节点n到终点的估计路径代价；再删除在/>中的节点，并将起点加入到/>中；
步骤S5.3:如果中没有节点，则执行步骤S5.6，否则，从/>中找到路径最小节点/>,并删除/>中的节点，并将该节点加入到/>中；
步骤S5.4：遍历搜索与节点相邻的节点，计算这些节点的路径长度，并将节点/>设置为父节点；搜索/>中与节点/>相邻节点，若存在节点并且以/>为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
所述步骤S6路径跟踪算法的控制效果取决于预瞄距离/>，如果/>越大，控制效果会越平滑，/>越小，控制的效果越精确。而/>与收割机当前的速度有关。其中具体的求取公式如下：
，
其中，,为收割机的最大制动减速度，表示收割机的最
短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最
小转弯半径。
通过上面的公式确定收割机的预瞄距离，可确定控制参数，使得该算法可以对收割机的行驶路径进行跟踪控制，最终让收割机可以按照规划的路径行驶。
所述路径行驶具体为在决策模块完成计算后会通过CAN分析仪向底层液压控制器发送CAN指令，收割机的底层液压控制器接收到CAN指令后通过电磁阀驱动收割机的液压阀从而驱动收割机的行走，实现自动行走功能。
本发明基于SLAM技术的收割机机耕道场景地图构建方法,针对收割机作业场景下的机耕道场景，采用激光雷达与GPS融合结合NDT点云配准算法构建出机耕道场景下的高精度点云地图，利用A*全局路径规划算法,规划了收割机机耕道下的最优行驶路径，并通过算法对规划的路径进行跟踪，使得收割机不偏离规划得路径。通过CAN总线向收割机底层液压控制器发送CAN指令，通过电磁阀驱动液压阀驱动收割机往前行驶，最终实现了在机耕道下的自动导航。
本发明适合收割机收割场景下的机耕道路的行驶，面对一些GPS信号比较弱的农田，利用激光雷达的SLAM技术，解决了GPS不稳定情况下的自动导航问题。另外，通过在点云地图构建的时候使用机耕道点云处理算法，使得本发明可以适用于机耕道场景下复杂路面情况造成的点云噪声过多的情形，从而构建满足机耕道场景的地图，实现收割机收割场景下的自动导航。
应当理解，虽然本说明书是按照各个实施例描述的，但并非每个实施例仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施例的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。
Claims (10)
1.一种基于SLAM技术的收割机机耕道自动导航方法，其特征在于，包括以下步骤：
步骤S1、系统初始化：对系统进行配置和初始化；
步骤S2、环境感知：获取多线激光雷达获取点云信息以及GPS的位姿信息；
步骤S3、地图构建：根据步骤S2获取的环境三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；
步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，完成全局地图构建；
步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；
步骤S6、路径跟踪：按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_ pursuit对规划好的路径进行跟踪。
2.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
3.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S3在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图具体包括以下步骤：
首先对机耕道上的噪声点云进行点云滤除，设t时刻的点云为,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为/>，动态目标的点云标签为/>，再对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组/>，如果属于动态目标类的点云，则移动到动态目标点云/>，通过以上的遍历，将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通过对机耕道上滤除后的点云的邻域进行插值，生成新的点，增加点云数据的密度，得到更清晰的机耕道的特征；
随后设置输入点云组和参考点云组，然后对输入点云组运动量做平移和旋转变换得到变换后的点云组，将变换后的点云组/>转换到参考点云组Q，根据下面公式得到点云的概率/>：
，
其中，为/>中的某个单元格，/>为参考点云组中/>单元格的平均值，/>为参考点云组中/>单元格的协方差；
将输入点云组中的n个点的概率求和，求其平均值作为配准分数，其计算公式如下：
，
其中，是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其满足迭代条件，并使得分最大，得到三维点云地图。
4.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S4地图约束具体包括以下步骤：
在三维点云地图中加入GPS信息，首先判断第i帧特征点云是否为关键帧，并且定位误差大于阈值/>,则根据间戳在GPS列表中查找最近的点在NDT算法中添加GPS约束，通过目标优化函数优化地图。
5.根据权利要求4所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述目标优化函数公式如下：
，
其中，为所有边的集合，/>为任意两个顶点的下标，/>为/>两点的误差，/>为/>的转置，/>为GPS观测的协方差矩阵，表示为：
，
其中，为GPS节点与其相邻最近的节点时间戳的差值。
6.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S5路径规划包括以下步骤：
步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合和/>,其中/>集合存放可能为最短路径的节点，/>已找出的最短路径的节点集合；
步骤S5.2：将起点放入中，遍历所有与起点相邻的节点，将符合条件的节点放入/>中，将起点设置为父节点，根据以下公式得到节点的路径长度：
，
其中，为节点的评估函数，/>为起点到节点n的路径代价，/>代表节点n到终点的估计路径代价；再删除在/>中的节点，并将起点加入到/>中；
步骤S5.3:如果中没有节点，则执行步骤S5.6，否则，从/>中找到路径最小节点/>,并删除/>中的节点，并将该节点加入到/>中；
步骤S5.4：遍历搜索与节点相邻的节点，计算这些节点的路径长度，并将节点/>设置为父节点；搜索/>中与节点/>相邻节点，若存在节点并且以/>为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；
步骤S5.5：查找目标节点e是否在中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；
步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
7.根据权利要求1所述的基于SLAM技术的收割机机耕道自动导航方法，其特征在于，所述步骤S6路径跟踪算法的控制效果取决于预瞄距离/>，/>具体的公式如下：
，
其中，,/>为收割机的最大制动减速度，/>表示收割机的最短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最小转弯半径。
8.一种实现权利要求1-7任意一项所述的基于SLAM技术的收割机机耕道自动导航方法的系统，其特征在于，包括感知模块、决策模块和执行模块；
所述感知模块包括多线激光雷达和GPS天线，多线激光雷达用于获取收割机周围的环境三维点云信息，GPS天线用于获取收割机的位姿状态；
所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；边缘计算平台用于处理获取到的三维点云信息，在点云配准算法NDT中加入机耕道点云处理算法，构建周围环境的点云地图；在点云地图中加入GPS约束，完成全局地图构建；根据构建的场景地图，使用A*算法对机耕道进行全局路径规划；采用执行模块，按照规划的路径进行行驶，在行驶的过程中，使用对规划好的路径进行跟踪；
所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。
9.根据权利要求8所述的基于SLAM技术的收割机机耕道自动导航系统，其特征在于，所述决策模块的边缘计算平台采用Linux系统的嵌入式开发板，嵌入式开发板安装在收割机驾驶室内，通过CAN总线与收割机底层液压控制器连接，通过网口与激光雷达连接，通过串口与GPS天线连接。
10.根据权利要求8所述的基于SLAM技术的收割机机耕道自动导航系统，其特征在于，所述执行模块通过收割机底层液压控制器控制液压系统电磁阀，驱动收割机行驶。
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 true CN116500638A (zh) | 2023-07-28 |
| CN116500638B 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)
| 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 | 华南农业大学 | 一种农田与机耕道边界线提取方法及系统 |
-
2023
- 2023-06-25 CN CN202310750642.2A patent/CN116500638B/zh active Active
Patent Citations (11)
| 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)
| Title |
|---|
| 朱世威: "无人驾驶农业机械导航系统研究", 《中国优秀硕士学位论文全文数据库农业科技辑》, no. 03 * |
| 王立辉 等: "智能收获机定位和自适应路径追踪方法", 《导航定位学报》, vol. 8, no. 6, pages 29 - 36 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116500638B (zh) | 2023-10-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103926925B (zh) | 一种基于改进的vfh算法的定位与避障方法及机器人 | |
| JP6928722B2 (ja) | 車両動作のための位置特定の決定 | |
| Kummerle et al. | Autonomous driving in a multi-level parking structure | |
| CN102368158B (zh) | 一种果园机械导航定位方法 | |
| CN107015238A (zh) | 基于三维激光雷达的无人车自主定位方法 | |
| Pfaff et al. | Towards mapping of cities | |
| CN110187372B (zh) | 一种低速无人车园区内组合导航方法及系统 | |
| CN109579861B (zh) | 一种基于强化学习的路径导航方法及系统 | |
| CN112859860A (zh) | 一种机器人系统及其路径规划方法 | |
| WO2023039666A1 (en) | Systems, methods, and media for occlusion-aware motion planning | |
| CN114034299A (zh) | 一种基于主动激光slam的导航系统 | |
| CN110213713B (zh) | 一种基于时空域联合处理的集中式协作定位系统和方法 | |
| CN114186112B (zh) | 一种基于贝叶斯优化多重信息增益探索策略的机器人导航方法 | |
| Srinara et al. | Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme | |
| 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 | |
| US11783178B2 (en) | Systems and methods for corridor intent prediction | |
| CN109752016A (zh) | 一种用于无人低速车的平行行驶航线轨迹生成系统 | |
| Qiu et al. | LiDAR point-cloud odometer based mobile robot routine tracking in orchards | |
| CN110645981B (zh) | 用于清洗桩基式水上光伏组件的无人船导航系统及方法 | |
| CN112051574A (zh) | 一种基于高精度地图的自动化旋耕船 | |
| Lee et al. | Autonomous Vehicle Localization Without Prior High-Definition Map | |
| CN118243127B (zh) | 一种轮式植保机器人的路径规划方法 | |
| CN114689037B (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 |