本发明专利技术提供了一种基于SLAM技术的收割机机耕道自动导航方法及系统，针对机耕道场景，采用激光雷达与GPS结合增加机耕道点云处理算法的NDT算法构建出机耕道场景下的高精度点云地图，利用A*全局路径规划算法,规划了收割机机耕道下的最优行驶路径，并通过Pure_pursuit算法对规划的路径进行跟踪，使得收割机不偏离规划得路径，通过CAN总线向收割机底层液压控制器驱动液压阀驱动收割机往前行驶，最终实现了机耕道下的自动导航。本发明专利技术适合收割场景下的机耕道路的行驶，面对一些GPS信号比较弱的农田，利用激光雷达的SLAM技术，解决了GPS不稳定情况下的自动导航问题。另外，通过机耕道点云处理算法消除点云噪声，提高地图构建精度，满足机耕道的自动导航需求。满足机耕道的自动导航需求。满足机耕道的自动导航需求。
【技术实现步骤摘要】
一种基于SLAM技术的收割机机耕道自动导航方法及系统
[0001]本专利技术属于自动导航
，尤其涉及一种基于SLAM技术的收割机机耕道自动导航方法及系统。
技术介绍
[0002]近年来，随着无人驾驶技术的不断发展，收割机的无人化程度不断提高。实现收割机自动驾驶功能可以节省劳动力，提供工作效率。
[0003]目前而言，收割机的自动导航都是单纯的依靠GPS实现的。这种导航方式的缺点是在GPS信号较弱或没有GPS信号时，无法实现自身的定位，无法进行路径规划，进而无法实现自动导航。如在机耕道路上，两侧的树木较多，GPS信号容易受到树木的干扰，另外收割机附近GPS基站信号的强弱，也会影响GPS信号的正常接收。若GPS信号受到影响，收割机在机耕道下的自动导航功能则无法正常实现。
[0004]SLAM又称同时建图和定位技术，通过该方法可构建点云地图同时实现定位功能，为自动导航服务。利用SLAM技术结合路径决策控制算法的导航方法相比与传统的GPS导航，有定位精度高和场景信息丰富的优点，不会受到机耕道两侧树木或者基站信号强度的影响。
技术实现思路
[0005]针对上述技术问题，本专利技术的一个方式的目的之一是提供一种基于SLAM技术的收割机机耕道自动导航方法，利用多线激光雷达获取收割机机耕道场境下的三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，并通过对机耕道路上点云处理和添加GPS约束，实现对机耕道场景的高精度点云地图的构建；地图构建完成后通过A*路径规划算法规划收割机的最优机耕道路径，并使用路径跟踪算法对规划路径进行跟踪，实现收割机在机耕道场景下的自动导航。本方法能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
[0006]本专利技术的一个方式的目的之一是提供一种基于SLAM技术的收割机机耕道自动导航系统，包括感知模块、决策模块和执行模块；所述感知模块包括多线激光雷达和GPS天线，用于获取收割机周围的环境信息以及收割机的位姿状态；所述决策模块包括边缘计算平台，用于处理获取到的三维点云信息，构建出机耕道的全局地图，并根据建图的结果规划出收割机最优机耕道行驶路径，且对收割机进行路径跟踪控制；所述执行模块包括收割机底层液压控制器，用于根据决策模块的跟踪控制指令驱动收割机行驶，使其按照规划好的路径行驶。能够有效解决收割机在无GPS信号或弱GPS信号的情况下的自动驾驶问题。
[0007]注意，这些目的的记载并不妨碍其他目的的存在。本专利技术的一个方式并不需要实现所有上述目的。可以从说明书、附图、权利要求书的记载中抽取上述目的以外的目的。
[0008]本专利技术是通过以下技术手段实现上述技术目的的。
[0009]一种基于SLAM技术的收割机机耕道自动导航方法，包括以下步骤：
步骤S1、系统初始化：对系统进行配置和初始化；步骤S2、环境感知：获取多线激光雷达获取点云信息以及GPS的位姿信息；步骤S3、地图构建：根据步骤S2获取的环境三维点云信息，在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图；步骤S4、地图约束：在步骤S3所述的点云地图中加入GPS约束，完成全局地图构建；步骤S5、路径规划：根据步骤S4构建的场景地图，使用A*算法对机耕道进行全局路径规划；步骤S6、路径跟踪：按照步骤S5规划的路径进行行驶，在行驶的过程中，使用Pure_pursuit对规划好的路径进行跟踪。
[0010]上述方案中，所述步骤S2环境感知具体通过多线激光雷达获取机耕道场景下的点云信息，通过GPS天线获取收割机的位置信息。
[0011]上述方案中，所述步骤S3在NDT点云配准算法中加入机耕道点云处理算法，构建周围环境的点云地图具体包括以下步骤：首先对机耕道上的噪声点云进行点云滤除，设t时刻的点云为,将地面类和动态目标类点云进行标记并设置其阈值，地面类点云的标签为，动态目标的点云标签为，再对点云进行遍历，如果该点云属于地面类的，则将该点云移动到地面点云组，如果属于动态目标类的点云，则移动到动态目标点云，通过以上的遍历，将地面和动态目标类的噪声点云过滤，得到去除噪点的机耕道点云数据，滤除机耕道上的噪声点云之后，使用点云上采样方法中的邻域插值方法，通过对机耕道上滤除后的点云的邻域进行插值，生成新的点，增加点云数据的密度，得到更清晰的机耕道的特征；随后设置输入点云组和参考点云组，然后对输入点云组运动量做平移和旋转变换得到变换后的点云组，将变换后的点云组转换到参考点云组Q，根据下面公式得到点云的概率：，其中，为中的某个单元格，为参考点云组中单元格的平均值，为参考点云组中单元格的协方差；将输入点云组中的n个点的概率求和，求其平均值作为配准分数，其计算公式如下：，其中，是点云组的运动量；得到配准分数之后，使用相应的优化算法优化目标函数，使其满足迭代条件，并使得分最大，得到三维点云地图。
[0012]上述方案中，所述步骤S4地图约束具体包括以下步骤：在三维点云地图中加入GPS信息，首先判断第i帧特征点云是否为关键帧，并且定位误差大于阈值,则根据时间戳在GPS列表中查找最近的点在NDT算法中添加GPS约束，通过目标优化函数优化地图。
[0013]进一步的，所述目标优化函数公式如下：
，其中，为所有边的集合，为任意两个顶点的下标，为两点的误差，为的转置，为GPS观测的协方差矩阵，表示为：，其中，为GPS节点与其相邻最近的节点时间戳的差值。
[0014]上述方案中，所述步骤S5路径规划包括以下步骤：步骤S5.1：根据步骤S4构建的全局地图，将地图划分为大小相等的网格，设有两个节点集合和,其中集合存放可能为最短路径的节点，已找出的最短路径的节点集合；步骤S5.2：将起点放入中，遍历所有与起点相邻的节点，将符合条件的节点放入中，将起点设置为父节点，根据以下公式得到节点的路径长度：，其中，为节点的评估函数，为起点到节点n的路径代价，代表节点n到终点的估计路径代价；再删除在中的节点，并将起点加入到中；步骤S5.3:如果中没有节点，则执行步骤S5.6，否则，从中找到路径最小节点,并删除中的节点，并将该节点加入到中；步骤S5.4：遍历搜索与节点相邻的节点，计算这些节点的路径长度，并将节点设置为父节点；搜索中与节点相邻节点，若存在节点并且以为父节点的路径长度小于原来的评估至，则更新该节点，否则，不更新；步骤S5.5：查找目标节点e是否在中，若存在，则执行步骤S5.6，否则，执行步骤S5.3；步骤S5.6：找出最短路径，结束遍历搜索，否则，也结束遍历搜索。
[0015]上述方案中，所述步骤S6路径跟踪算法的控制效果取决于预瞄距离，具体的公式如下：，其中，,为收割机的最大制动减速度，表示收割机的最短制动距离，B为收割机遇到异常时需要的反应时间，Bv为收割机的反应距离，C为车辆的最小转弯半径。
[0016]一种实现上述基于SLAM技术的收割机机耕道自动导航方法的系统，包括感知模块、决策模块和执行模块；所述感知模块包括多线激光雷达和GPS天线，多线激光雷达用于获取收本文档来自技高网...
【技术保护点】
【技术特征摘要】
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技术...
【专利技术属性】
技术研发人员：徐立章，戴步旺，刘朋，胡金鹏，崔志鸿，李启滨，
申请(专利权)人：江苏大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。