一种基于非对称折返的轮式无人农机地头调头路径动态规划方法与流程
本发明涉及农业机械自动导航与路径规划,具体涉及一种基于非对称折返的轮式无人农机地头调头路径动态规划方法。
背景技术:
1、轮式无人农机是实现农业生产自动化、精准化与少人化作业的重要装备形式,可广泛应用于播种、施肥、植保等农业生产环节。在实际田间作业过程中,轮式无人农机通常沿作物行或预设作业路径往复行驶,当轮式无人农机到达田块端部的地头区域后,需要完成调头并准确进入相邻作业行或目标作业行继续作业。地头调头虽属于非作业行驶过程,但其轨迹规划效果直接影响调头时间、行驶距离、入行横向偏差和航向衔接质量。特别是在地表起伏、土壤松软、轮辙明显及附着条件变化等复杂田间工况下,若调头路径规划不合理,容易造成入行不准、后续直线作业纠偏距离增加,进而引发重叠作业、漏作、压苗、行距不一致等问题,影响无人农机连续作业效率和整体作业质量。
2、目前,轮式无人农机地头调头方式主要包括半圆调头、u形调头、灯泡形调头和折返调头等。当地头空间充足、作业行间距较大时,轮式无人农机可采用半圆调头或u形调头完成行间转换;而在地头空间受限、作业行距较小或轮式无人农机最小转弯半径较大的情况下,轮式无人农机难以一次性转入目标作业行,通常需要采用前进、倒退、再前进的折返式调头方式。折返式调头能够降低对地头空间和作业行间距的要求,适用于狭窄地头区域,但其轨迹规划对折返点位置、倒车轨迹、入行轨迹以及轮式无人农机姿态衔接均有较高要求。此外,现有农机地头调头轨迹通常在轮式无人农机进入地头前或调头初始阶段生成,随后轮式无人农机按照既定轨迹进行跟踪。在理想地面条件下,该类方式能够完成基本调头任务;但在实际农田地头区域,轮式无人农机在低速、大转角、倒车和换向过程中容易发生侧滑、横向偏移和航向偏差,使实际行驶轨迹偏离预设轨迹。若轮式无人农机偏离后仍以回追原轨迹为主,容易增加调头距离和轨迹调整幅度,并可能导致进入目标作业行时横向偏差过大,影响后续作业路径的稳定衔接。
3、针对农业机械地头调头路径规划与控制问题,现有技术已从作业模式选择、曲率连续路径生成、宽幅作业转弯和实时路径更新等方面开展了相关探索。例如,cn106647770a根据农机作业模式选择对应掉头路径,并结合预瞄与pi控制实现自动掉头;cn112363504b采用交叉型调头路径并引入欧拉螺旋,以提高路径曲率连续性;cn112904867b面向宽幅作业农机生成地头转弯路径,以减少重叠和漏作;cn113406960a通过实时获取轮式无人农机位置和航向信息,生成以当前位置为起点的调头规划路线。上述技术虽然能够在一定程度上改善农机地头调头效果,但对狭窄地头、行距受限和侧滑扰动条件下折返调头轨迹的动态调整仍有进一步优化空间,尤其需要从轨迹规划层面综合考虑轮式无人农机实时位姿、前进转弯约束、倒车转弯约束和目标作业行入行约束。
4、综上所述,现有轮式无人农机地头调头轨迹规划技术虽可实现一定条件下的自动调头,但在狭窄地头和侧滑扰动等复杂田间工况下,仍存在折返轨迹对轮式无人农机实时位姿适应性不足、调头路径较长、入行偏差较大的问题。因此,本发明提出一种基于非对称折返的轮式无人农机地头调头路径动态规划方法,根据轮式无人农机当前位置和航向实时更新折返点及后续入行轨迹,以提高狭窄地头条件下的调头效率、入行精度和作业行衔接稳定性。
技术实现思路
1、本发明所要解决的问题是现有轮式无人农机地头调头轨迹规划方法在狭窄地头、行距受限及侧滑扰动等田间工况下,存在调头轨迹对轮式无人农机实时位姿适应性不足、折返点位置难以及时调整、调头路径较长以及进入目标作业行时横向偏差较大的问题。
2、为解决上述问题,本发明实施例中提供了一种基于非对称折返的轮式无人农机地头调头路径动态规划方法,该方法包括以下步骤:
3、s1、轮式无人农机状态与地头作业参数获取;
4、所述轮式无人农机状态包括当前实时位置、航向角和行驶速度;
5、所述地头作业参数包括当前作业行、目标作业行、作业行距以及目标入行位置;
6、s2、非对称折返调头路径生成;首先定义调头起点、第一折返点和第二折返点以及目标入行点,构建由第一前进圆弧段、倒车圆弧段和第二前进入行圆弧段组成的三段式折返路径;
7、s3、基于实时位姿的调头路径动态更新;根据轮式无人农机实时位置和航向角,对折返点和后续轨迹进行动态更新;
8、所述轨迹动态更新作用于第一前进圆弧段和倒车圆弧段,在实际轨迹与原始固定轨迹出现侧滑、横向偏移或航向偏差后,以当前实际位姿为新的起点重新生成后续调头轨迹;
9、s4、目标作业行入行与调头完成判断;所述目标作业行入行判断,执行第二折返点由倒车状态切换为前进状态,根据第二前进入行圆弧段,圆弧半径生成面向目标作业行的入行圆弧路径,构成入行圆弧轨迹点集合,根据预瞄距离从所述轨迹点集合中选取目标点,计算轮式无人农机转向控制量;
10、所述调头完成判断,执行入行,实时计算轮式无人农机相对于目标作业行的横向偏差和航向偏差;判断所述横向偏差和航向偏差是否均小于预设阈值,若小于判定轮式无人农机完成地头调头,执行目标作业行直线作业路径跟踪状态。
11、进一步地,所述轮式无人农机状态与地头作业参数获取,还包括以下步骤:
12、s101、轮式无人农机实时位姿与转弯约束获取;获取轮式无人农机的实时位姿信息和轮式无人农机机械运动参数以及获取当前作业基础参数;
13、s102、作业行参数获取与地头局部坐标系建立;建立地头局部坐标系,将轮式无人农机实时位姿信息和机械运动参数以及当前作业基础参数统一表示。
14、进一步地,所述非对称折返调头路径生成,还包括以下步骤:
15、s201、调头关键点与轨迹段定义;建立地头局部坐标系,设定轮式无人农机当前所在位置定义为调头起点,轮式无人农机由前进转为倒车的位置定义为第一折返点,轮式无人农机由倒车转为前进的位置定义为第二折返点,轮式无人农机进入目标作业行的位置定义为目标入行点;
16、s202、非对称三圆弧轨迹构建;轮式无人农机转弯约束状态,取第一前进圆弧段的转弯半径和倒车圆弧段的转弯半径,根据轮式无人农机当前位姿、第二折返点和目标入行点的几何关系动态确定第二前进入行圆弧段的转弯半径;
17、s203、折返点坐标计算;建立目标入行点的局部坐标系原点,定义目标作业行方向为纵向参考方向,轮式无人农机当前位置到达定义的调头起点,计算第一折返点和第二折返点的坐标;
18、所述计算第一折返点坐标:
19、,其中,轮式无人农机坐标、航向角、前进最小转弯半径、倒车最小转弯半径以及圆心角、;
20、所述计算第二折返点坐标:
21、;
22、s204、入行圆弧半径与轨迹长度求解;计算得出第二前进入行圆弧段的转弯半径:
23、,其中,轮式无人农机坐标、航向角、前进最小转弯半径、倒车最小转弯半径以及圆心角、;
24、根据第二前进入行圆弧段的转弯半径,计算得出当前非对称折返调头轨迹的总长度:
25、,其中,前进最小转弯半径、倒车最小转弯半径以及圆心角、;
26、s205、当前调头轨迹输出;获取计算得出的第一折返点和第二折返点以及入行圆弧半径,计算输出当前轮式无人农机位姿下的非对称折返调头轨迹;
27、所述非对称折返调头轨迹由第一前进圆弧段和倒车圆弧段以及第二前进入行圆弧段构成,用于引导轮式无人农机从当前作业行转入目标作业行。
28、进一步地,所述基于实时位姿的调头路径动态更新,还包括以下步骤:
29、s301、第一前进圆弧段实时更新;执行第一前进圆弧段,定义当前控制周期轮式无人农机位置为,航向角为,设定作为当前轨迹起点,生成非对称折返轨迹,对第一折返点、第二折返点以及后续入行圆弧参数重新求解;
30、s302、第一折返点到达判断与换向控制;计算轮式无人农机当前位置与当前第一折返点之间的距离,若大于预设阈值,轮式无人农机继续执行第一前进圆弧段,并持续更新第一折返点和后续轨迹,若不大于预设阈值,判定轮式无人农机到达第一折返点,输出换向指令,使轮式无人农机由前进状态切换为倒车状态,进入倒车圆弧段;
31、s303、倒车圆弧段实时更新;定义倒车状态下当前轮式无人农机位置为,航向角为,倒车最小转弯半径为,倒车圆弧对应圆心角为,根据轮式无人农机当前位姿和倒车圆弧几何关系计算得到当前第二折返点:
32、,
33、所述当前第二折返点根据每个规划周期内的轮式无人农机实际位姿更新;
34、s304、第二折返点到达判断与入行阶段切换;计算轮式无人农机当前位置与当前第二折返点之间的距离,若大于预设阈值,执行倒车圆弧段,并持续更新第二折返点和后续入行轨迹;若不大于,判定轮式无人农机到达第二折返点,输出前进控制指令,使轮式无人农机由倒车状态切换为前进状态,进入目标作业行入行阶段。
35、进一步地,所述目标作业行入行与调头完成判断,还包括以下步骤:
36、s401、入行圆弧轨迹点生成;根据计算得出的第二前进入行圆弧半径,生成入行圆弧轨迹;
37、所述入行圆弧以目标入行点为终点,并使轮式无人农机在接近目标入行点时航向逐渐与目标作业行方向一致;
38、所述第二前进入行圆弧半径,根据当前位置为,以及设定目标入行点局部坐标系原点,计算得出:
39、,
40、所述入行圆弧半径,沿第二折返点到目标入行点圆弧生成一组离散轨迹点:
41、,其中,表示入行圆弧轨迹点集合,表示第个待跟踪轨迹点
42、s402、基于预瞄距离的入行轨迹跟踪;选取距离轮式无人农机当前位置满足预瞄距离要求的轨迹点作为当前目标点;
43、根据几何跟踪关系计算轮式无人农机前轮转角:
44、,其中,表示轮式无人农机轴距、预瞄距离;
45、所述预瞄距离根据轮式无人农机行驶速度动态确定;
46、s403、调头完成判断与后续路径输出;实时计算轮式无人农机相对于目标作业行的横向偏差和航向偏差。判断横向偏差是否小于预设横向阈值,且满足航向与目标作业行方向之间的航向偏差小于预设航向阈值,则判定轮式无人农机已完成地头调头并进入目标作业行。
47、进一步地,所述非对称折返调头轨迹的总长度,以轨迹长度较短为目标,满足当前位置经第一折返点和第二折返点到达目标入行点和目标入行点附近与目标作业行方向一致的条件,计算求解、、和,确定当前轮式无人农机位姿下的第一折返点、第二折返点和第二前进入行圆弧段参数。
48、与现有技术相比,本发明一方面可基于轮式无人农机调头时实时位置与航向信息动态生成适配当前位姿的非对称折返路径,轮式无人农机出现侧滑、横向偏移或航向偏差时无需大幅回追修正原有固定轨迹,轨迹自适应能力更强;另一方面,采用可在线规划折返点与入行轨迹的非对称折返调头方案,适配地头空间狭小、作业行距小、轮式无人农机转弯半径受限的田间工况,能够有效缩短调头行驶路径、压缩非作业耗时;同时,依托目标作业行为约束生成入行轨迹,可显著降低农机驶入目标作业行的横向偏差,提升行间作业衔接稳定性,减少后续直线作业阶段的轨迹纠偏工作量,综合提升无人农机田间自主作业精度与作业效率。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com