一种玉米田间机器人车的导航控制方法及系统

本发明涉及导航，具体涉及到农业机器人的导航。

背景技术：

1、随着农业现代化的发展，农业机器人在农田作业中的应用越来越广泛。在农作物种植过程中，根据不同农作物的生物学特征不同，以及农作物生长不同阶段，对于农业机器人的功能要求也不同。

2、在玉米等高秆作物在生长中后期会形成明显的冠层遮挡，导致农田无人运动平台在冠下行驶时面临较强的感知与控制耦合问题。与开阔环境相比，冠下场景通常具有以下特点：

3、卫星定位信号容易受到冠层遮挡和多路径效应影响，难以长期稳定地用于行间厘米级导航。视觉传感器容易受到叶片遮挡、阴影、光照变化和纹理重复的影响，在成熟期冠下环境中稳定性不足。激光雷达虽然能够提供相对稳定的结构信息，但在冠下仍会受到叶片散乱点、缺株断行、多行同时进入视场、局部点云稀疏等因素干扰。行间环境并不是固定宽度的理想通道，走廊宽度会随植株姿态、叶片侵入和局部缺失不断变化。

4、尤其针对玉米中后期，鉴于与米植株的特点使得在冠层下，叶片下垂/外展会周期性侵入行间有效通行宽度，进一步，近地茎秆、叶片和地表杂点会造成局部点云支持不足导致检测数据中出现缺株、断行及传感器视场内多行并存等问题，进而导致提取的中心线出现短时跳变的情况，该情况会导致车辆停车；进一步，玉米地的土壤松软和垄沟不平的实际情况也会导致车体在行进过程中产生摆动，进而产生横向余量，有可能会与近地茎秆、叶片碰撞影响车体前行的稳定性。

技术实现思路

1、本发明解决了现有玉米田间导航技术中存在的由于玉米植株顶部遮挡、近地茎秆和叶片的阻挡以及垄沟的影响，导致提取的中心线出现短时跳变导致车辆停车，以及受玉米地的土壤松软和垄沟不平的实际情况会导致车体在行进过程中产生摆动，进而产生横向余量使得导航路径无法顺畅通行的问题。

2、本发明所述的一种玉米田间机器人车的导航控制方法是针对所述方法的导航控制对象是玉米田间作业的机器人车，所述方法包括：

3、环境感知步骤：采集冠下传感器数据，包括激光雷达点云数据以及机器人车位姿信息；并将所述点云数据投影到地面获得相应的二维数据的投影点云数据；

4、中心线提取步骤：根据位姿信息和投影点云数据中提取左右作物行内侧边界，并生成行间通行走廊的中心线；

5、感知质量评估步骤：计算走廊置信度和安全裕度；

6、路径跟踪控制步骤：在中心线上选取目标路径点；

7、根据中心线、横向误差、航向误差、走廊置信度和安全裕度生成控制指令。

8、优选地，在中心线提取步骤之前，还包括对所述点云数据进行预处理的步骤。

9、优选地，对所述点云数据进行预处理为：

10、对点云数据进行高度约束和感兴趣区域约束，得到冠下候选点集：

11、，

12、其中，为待处理的点云数据，为：

13、，

14、、、、、、分别表示感兴趣区域的三维坐标的最大值和最小值，作为区域约束条件；

15、将所述候选点集投影到地面平面，获得投影点云数据：

16、。

17、优选地，所述左右作物行内侧边界提取包括：

18、将投影点云数据按横向分布进行聚类，得到若干候选作物行簇：

19、，

20、在机器人车的左右两侧分别选择距离车辆中心最近、且满足支持点数量阈值的内侧行，其中左侧行和右侧行分别为：

21、，

22、其中为一侧的候选作物行簇的索引，为第个候选作物行簇的横向中心。

23、优选地，所述行间通行走廊的中心线为：

24、，

25、所述和分别为走廊行方向上的左右行横向位置，是根据左右边界估计获得，为走廊中心线的横向位置，

26、，

27、为走廊中心线与作物行方向夹角，为沿作物行方向的路径参数。

28、优选地，所述走廊置信度为：

29、由至少两种信度加权得到：

30、，

31、其中：

32、，

33、所述置信度为：点云数量置信度、行距合理性置信度、中心线连续性置信度或历史稳定性置信度。

34、优选地，所述安全裕度为：

35、，

36、其中，为机器人车上的平台宽度，为当前走廊宽度。

37、优选地，所述安全裕度还可以为：

38、，

39、为玉米叶片接触容忍度或安全缓冲距离。

40、优选地，在中心线上选取目标路径点为：

41、；

42、为在中心线上、距离机器人车当前位置最近的路径参数：

43、，

44、为沿中心线向前选取的前视距离。

45、本发明还所述的一种玉米田间机器人车的导航控制系统用于对玉米田间作业的机器人车实现导航控制，所述系统包括：

46、环境感知模块：用于采集冠下传感器数据，包括激光雷达点云数据以及机器人车位姿信息；还用于将所述点云数据投影到地面获得相应的二维数据的投影点云数据;

47、中心线提取模块：用于根据位姿信息，以及二维数据的投影点云数据中提取左右作物行内侧边界，并生成行间通行走廊的中心线；

48、感知质量评估模块：计算走廊置信度和安全裕度；

49、路径跟踪控制模块：用于根据中心线、横向误差、航向误差、走廊置信度和安全裕度生成控制指令；

50、跟踪执行模块，用于将所述控制指令转换成执行机构驱动信号输出。

51、优选地，所述系统还包括：机器人车和环境传感器，所述环境传感器用于采集机器人车所在环境的激光雷达点云数据以及机器人车位姿信息，并发送给环境感知模块；所述跟踪执行模块输出的执行机构驱动信号发送至机器人车的运动执行机构。

52、本发明提出一种全新的设计构思，将中心线质量和可通行空间参引入路径跟踪控制的控制决策所实现的闭环控制技术，在获得中心线之后，继续计算中心线置信度和通道安全裕度，并用二者分级约束线速度和角速度，在低置信度的有限帧内调用历史可靠中心线，即本发明不再默认参考路径始终可靠，具体优点包括：

53、1、本发明充分考虑玉米田间的植株高度、密度以及土壤的特点，通过中心线置信度和通道安全裕度来确定“中心线可信度”和“剩余通行空间”，基于二者共同决定保守控制、短时恢复或者停车的控制方式。有效避免了当导航前方的通行走廊宽度变窄时，由于无法根据剩余通行空间主动降速或收缩控制导致的行驶故障。

54、2、本发明能够识别“中心线存在但质量不稳定”的过渡状态，从而在完全停车之前先进入低速降级跟踪，明显改善连续通行能力。

55、3、本发明能够根据走廊安全裕度动态约束线速度和角速度，在通道变窄时主动采取保守控制，降低碰撞风险。

56、4、本发明能够在中心线短时缺失或局部跳变时，调用历史可靠中心线继续进行导航，有效减少了由于瞬时感知异常造成的误停车、误转向、偏离当前行间和控制抖动的问题。

57、5、本发明的优势不依赖于某一种特定控制器。即使后续将 pid 替换为 purepursuit、stanley 或其他控制器，只要仍使用走廊置信度和安全裕度约束控制闭环。

58、本发明适用于机器人在冠下复杂环境中的导航控制，在保证顺利导航行驶的前提下，还能在风险增大时主动保守控制，所述冠下复杂环境例如：植物冠层遮挡明显、行间狭窄、点云局部不稳定的场景。