CN114047755B

CN114047755B - 农药喷洒机器人导航规划方法、计算机装置

Info

Publication number: CN114047755B
Application number: CN202111299505.9A
Authority: CN
Inventors: 余伶俐; 罗嘉威; 吴汉钊; 许泽中; 周开军
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2021-11-04
Filing date: 2021-11-04
Publication date: 2023-12-19
Anticipated expiration: 2041-11-04
Also published as: CN114047755A

Abstract

本发明公开了一种农药喷洒机器人导航规划方法、计算机装置及程序产品，根据机器人定位信息选取与机器人距离最近的车道线，生成引导路径参考点；机器人生成基于高精地图车道线的全局路径，待机器人抵达棚内主路后，自适应切换至任务导航模式，开始农药喷洒任务；通过机载激光雷达采集到的环境信息生成垄间行驶地面参考点以及避碰参考点，地面参考点可以保证机器人在垄间行驶时不会因为垄间地形的原因而发生侧翻，避碰参考点可以保证机器人在垄间主路行驶时避开生长茂盛的农作物以免造成损害，保证了机器人在垄间行驶的过程中在不会发生侧翻的同时，能够避开生长过于茂盛的农作物。

Description

农药喷洒机器人导航规划方法、计算机装置

技术领域

本发明涉及一种智能机器人路径规划领域，特别是一种农药喷洒机器人导航规划方法、计算机装置及程序产品。

背景技术

智慧农场内农药喷洒机器人导航规划是智能机器人导航规划领域的重要研究内容之一，结合高精地图车道线导航可以保证机器人在从仓库行驶至棚内的过程中更加顺畅，而结合激光雷达实现垄间实时导航可以极大地保证棚内农作物的安全无损，具有重要的研究价值。实现智慧农场内农药喷洒机器人导航规划首先需要根据机器人定位信息选取距离机器人最近的车道线，再根据机器人定位信息和最近车道线信息生成车道线引导路径参考点，将机器人引导至车道线上，进而生成基于高精地图车道线的全局路径，导航至棚内主路；基于车道线的导航保证了机器人行驶更加稳定顺畅。机器人抵达棚内主路农药喷洒任务点后，自适应切换至任务导航模式，结合激光雷达采集的环境信息进行实时导航，生成垄间行驶地面参考点和垄间行驶避碰参考点，保证了机器人在垄间行驶的过程中在不会发生侧翻的同时能够避开生长过于茂盛的农作物，降低对农作物的损害；同时结合激光雷达采集的航向角以及距离信息实现对垄间直角弯道的判断，实现机器人自主遍历垄间道路，实现对垄间农作物无遗漏作业；同时机器人能够自主判断农药喷洒任务完成度，当农药喷洒任务完成后，机器人会自适应切换回目标导航模式，生成基于高精地图车道线的全局路径回到仓库，等待下一次农药喷洒任务点的发布。然而，在实际场景中，仍有以下问题制约农药喷洒机器人在智慧农场内的应用。

(1)采用高精地图除了可以为机器人提供高精度的静态信息外，还可以结合传感器信息实现障碍物动态更新；考虑到机器人起始位置可能会偏离车道线，如果不将机器人引导至车道线上会影响后面基于高精地图的巡逻导航；发明专利申请《移动机器人混合导航方法、设备及存储装置》(专利申请号：202010592745.7)公开了一种移动机器人混合导航方法、设备及存储装置，该方法需要人为的在现实场景中绘制车道线，而后通过摄像头进行车道线识别，再而进行车道线导航；若未识别到车道线，则切换至利用激光雷达扫描方式先进行建图，再在栅格地图上人为设置相应的导航模式切换点进行切换、导航，该发明可以在上下坡、长走廊及夜间环境进行导航，但难以应用于农场环境中，该方法需要人力进行实际场景的车道线绘制，同时需要人为设置导航模式切换点，在自动化以及智能化程度上有所不足。而本发明所述的车道线导航不需绘制实际场景车道线即可达到同样的效果，通过人工绘制高精地图用于机器读取，可以实现任意的修改，在操作上更加便利、在程序运行资源占用上更少并且运行时间更快。

(2)采用机器人替代人工执行农药喷洒任务是必然趋势，机器人既能替代人工完成日常农作物护理任务，同时能够实现24小时不间断作业，并且能够通过搭载摄像头等传感器帮助用户更好的监视农场情况；但是通常的导航规划算法针对机器人在棚内作业是行不通的，机器人在棚内的GPS精度不足，贸然使用机器人进行垄间作业可能存在风险；通过设计一种结合激光雷达传感器信息实现垄间实时导航算法可以降低该风险甚至避免，用户也无需担心机器人会碰撞到农作物等危险情况；发明专利申请《一种集群机器人协同定位系统及方法》(专利申请号：202011490718.5)公开了一种集群机器人协同定位系统及方法，该方法采用主机器人构建地图，通过主机器人向从机器人共享地图，实现主机器人以及从机器人共同基于主机器人构建的地图进行导航定位，但该方法在棚内实现难度高，由于棚内农作物生长的不规律以及棚内定位精度较差，无法建立一个精度较高的地图。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种农药喷洒机器人导航规划方法、计算机装置及程序产品，不需实际场景车道线即可达到与车道线导航同样的效果。

为解决上述技术问题，本发明所采用的技术方案是：一种农药喷洒机器人导航规划方法，包括以下步骤：

S1、根据机器人定位信息判断离机器人当前位置最近的车道线；

S2、根据机器人定位信息以及最近车道线信息生成车道线引导路径参考点；

S3、机器人跟踪车道线引导路径参考点行驶，当机器人抵达棚内主路时，采集棚内环境信息，生成机器人垄间行驶地面参考点；

S4、根据所述环境信息生成垄间行驶避碰参考点，结合垄间行驶避碰参考点和垄间行驶地面参考点生成最佳行驶参考点；

S5、机器人根据最佳行驶参考点行驶时，根据环境信息获取机器人距离垄间尽头墙面的距离，进行垄间直角弯道的判断，完成农药喷洒任务完成度的判断。

本发明可以采用激光雷达实时探测到的点云数据进行导航，无需建图过程，操作流程上更加简便；同时本发明的导航模式切换点也不需要人为设置，通过机器人自主判断实现模式切换，在自动化以及智能化上程度更高。本发明可以采用激光雷达实时数据进行导航，在避开建图这一大难点的同时实现了导航的功能，在定位精度不高的棚内进行导航作业，可以根据激光雷达扫描得到的点云数据实现垄间遍历导航，在直线行驶的同时可以判断是否需要转向，智能化程度更高，并且不需要多机器人协同定位，成本更低。

步骤S1的具体实现过程包括：

1)读取棚内农药喷洒起始点坐标文件，将棚内农药喷洒起始点Point_spraying,a存入农药喷洒点点集G_spraying＝{Point_spraying,a|a＝1、2、…、Num_spraying}中，其中Num_spraying表示农药喷洒点总数；记录仓库坐标G_warehouse；

2)选定某一个农药喷洒点Point_spraying，机器人从仓库出发，导航模式为目标导航模式，记为Mode＝1，目标点为农药喷洒点Point_spraying；

3)遍历所有的车道线Lane_i，其中i表示车道线编号；判断在第i条车道线Lane_i是否存在轨迹点Point_ij，其中j表示车道线Lane_i的第j个轨迹点，j＝1,6,…,(5k+1)；k为大于0的正整数；若存在，则转到步骤4)；若不存在，则转到步骤5)；

4)选取车道线Lane_i的轨迹点Point_ij，并计算轨迹点Point_ij距离机器人当前位置Point_current的距离distance_ij，将距离distance_ij以及对应的车道线编号i分别存入集合Distance_all＝{distance_ij|j＝1、6、…、(5k+1)}和集合Index_all＝{i|i＝1、2、…}中；(x_ij,y_ij)和(x_c,y_c)分别是轨迹点Point_ij和机器人当前位置Point_current的坐标；

5)从集合Distance_all中选取来自两条不同车道线的最短距离distance_min1、distance_min2，并在集合Index_all中对应选取该两条不同车道线的编号i_min1、i_min2；

6)遍历车道线编号为i_min1、i_min2的两条车道线Lane_min1、Lane_min2上的所有轨迹点Point_min,bd，并计算轨迹点Point_min,bd距离机器人当前位置Point_current的距离distance_min,bd，将距离distance_min,bd以及对应的车道线编号i_min分别存入集合Distance_min＝{distance_min,bd|b＝1、2，d＝1、2、…、num_b}和集合Index_min＝{b|b＝1、2}中，其中num_b表示编号为b的车道线轨迹点数量；distance_min,bd的计算公式：(x_min,bd,y_min,bd)和(x_c,y_c)分别是轨迹点Point_min,bd和机器人当前位置Point_current的坐标。

考虑到机器人起始位置存在偏离车道线的情况，本发明根据机器人当前位置进行最近车道线的判断以便于后续车道线引导路径的生成，并且可以快速导航至距离最近的车道线，节约时间，提高了导航效率。

步骤S2的具体实现过程包括：

步骤1，定义引导路径参考点数量Num_{guidepathpoint}＝0；

步骤2，在集合Distance_min中查找最短距离并记为distance_temporarymin，根据最小距离distance_temporarymin对应查找distance_temporarymin在车道线Lane_temporarymin上的轨迹点，该轨迹点即为距离机器人当前位置最近的轨迹点Point_{temporarycurrentmin}，同时查找轨迹点Point_{temporarycurrentmin}所在车道线的编号i_temporarymin，沿着车道线前进方向计算从轨迹点Point_{temporarycurrentmin}至车道线末尾轨迹点Point_{temporarycurrentend}的数量Num_{pointtemporary}；

步骤3，判断是否满足Num_{pointtemporary}≤50；若满足，则在集合Index_min＝{b|b＝1、2}中找到i≠i_temporarymin的车道线编号并记为i_min，对应的车道线记为Lane_min，转到步骤4；若不满足，则i_min＝i_temporarymin、Point_currentmin＝Point_{temporarycurrentmin}、Lane_min＝Lane_temporarymin，转到步骤5；

步骤4，判断是否已找到轨迹点Point_currentmin；若已找到，则转到步骤6；若未找到，则遍历车道线Lane_min上所有的轨迹点Point_j，找到距离机器人当前位置最近的轨迹点Point_currentmin，转到步骤5；

步骤5，判断是否满足Num_{guidepathpoint}≤10；若满足，则distance_temporary＝distance_currentmin、引导路径参考点数量Num_{guidepathpoint}加1，转到步骤6；若不满足，则计算distance_temporary＝distance_currentmin-0.2×(Num_{guidepathpoint}-10)、引导路径参考点数量Num_{guidepathpoint}加1，判断是否满足distance_temporary≥0；若满足，则转到步骤6；若不满足，则说明机器人已抵达车道线上；

步骤6，在车道线Lane_min上朝着目标点方向选取点Point_currentmin的下一个轨迹点Point_currentmin+1，获取Point_currentmin+1的位置信息(x_currentmin+1,y_currentmin+1)，并计算从Point_currentmin到Point_currentmin+1的方向角α，计算机器人当前位置Point_current至轨迹点Point_currentmin的方向角β；

步骤7，判断方向角α和方向角β之间的角度差是否满足75°≤|α-β|≤105°；若满足，则转到步骤10，并且记Point_current′＝Point_current为距离轨迹点Point_currentmin最近的坐标点，记录插值标志位flag_expand＝0；若不满足，则转到步骤8，并记flag_expand＝1；

步骤8，根据机器人当前位置Point_current以及方向角α计算经过机器人当前位置且平行于车道线Lane_min的一条平行线方程l：y＝tan(α)·(x-x_current)+y_current，其中(x_current,y_current)是机器人当前位置Point_current的坐标，tan(α)是平行线的斜率；

步骤9，在平行线l上寻找距离轨迹点Point_currentmin最近的点Point_lmin，并使得Point_current′＝Point_lmin；

步骤10，若插值标志位flag_expand＝1，则采用三次样条插值法从机器人当前位置Point_current至点Point_current′进行插值，并将插值点坐标存入参考点集合Reference_guidepath中，并转到步骤11；若flag_expand＝0，则Point_current′＝Point_current，转到步骤11；

步骤11，计算引导路径参考点Point_reference，并存入车道线引导路径参考点集合Reference_guidepath中，具体计算式如下：

其中，(x_currentmin+1,y_currentmin+1)是点Point_currentmin+1的坐标，(x_reference,y_reference)是参考点Point_reference的坐标；distance为轨迹点Point_current′与车道线之间的距离。

本发明通过引导路径参考点迅速将机器人引导至最近的车道线上，进而生成基于高精地图车道线的全局路径，在行驶上更加稳定顺畅，并且在计算机资源上占用更少。

distance的确定过程包括：

计算车道线Lane_min上的两个轨迹点Point_currentmin和Point_currentmin+1的直线方程：(y_currentmin+1-y_currentmin)x-(x_currentmin+1-x_currentmin)y+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＝0；其中，(x_currentmin,y_currentmin)和(x_currentmin+1,y_currentmin+1)分别为点Point_currentmin和Point_currentmin+1的坐标；将轨迹点Point_current′的坐标带入直线方程中进行判断，若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＞0，则说明Point_current′在车道线的右侧，distance＝distance_temporary；若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＜0，则说明Point_current′在车道线的左侧，distance＝-distance_temporary。

轨迹点Point_current′与车道线之间的距离的确定能够更加方便的生成引导路径参考点，并保证参考点的计算准确。

步骤S3的具体实现过程包括：

A)机器人抵达至车道线上，根据机器人定位信息和农药喷洒目标点坐标生成基于地图车道线的全局路径，机器人根据生成的全局路径抵达农药喷洒目标点，该喷洒点位于棚内主路上，切换至任务导航模式，记为Mode＝2，记iteration_cloud＝0；建立机器人坐标系，该坐标系以机器人的质心在地面上的投影为原点，机器人前进方向为y轴正方向，机器人右侧为x轴正方向，上方为z轴正方向；

B)机器人左转90°，激光雷达采集到的点云数据相对机器人的坐标为(x,y,z)；

C)获得一帧点云数据，更新iteration_cloud加1，将点云数据按照y≤0和y＞0进行分类，分别记作Cloud_left和Cloud_right；

D)分别选取集合Cloud_left和Cloud_right中满足以下两个条件的点云并存入集合Cloud_leftdemand和Cloud_rightdemand中：与平面S_planey:y＝0.2×iteration_cloud的距离distance_planey≤K1、与平面S_planez:z＝K2的距离distance_planez≤K1；K1、K2为设定的阈值；

E)分别在集合Cloud_leftdemand和Cloud_rightdemand中找到机器人坐标系横坐标值最接近于0的点云数据，分别记作Point_{leftreference}和Point_{rightreference}，Point_{leftreference}和Point_{rightreference}坐标分别为(x_{leftreference},y_{leftreference},z_{leftreference})、(x_{rightreference},y_{rightreference},z_{rightreference})；

F)计算垄间行驶参考点Point_{temporaryreference}(x_{temporaryreference}，y_{temporaryreference}，z_{temporaryreference})，并存入机器人垄间行驶地面参考点集合Reference_{temporaryridge}，具体计算式如下：

G)清空集合Cloud_leftdemand和Cloud_rightdemand。

考虑到棚内垄间道路两边呈凹形，如图6，垄间行驶地面参考点的生成可以防止机器人在行驶过程中发生侧翻。

步骤S4的具体实现过程包括：

A)在集合Cloud_left中找到z值最大的点云，记为Point_leftzmax；在集合Cloud_right找到z值最大的点云，记为Point_rightzmax；

B)选取平面高度h_crops，其中h_crops＝(min{z_leftzmax,z_rightzmax,z_robot}+h)/2，z_robot为机器人在z轴上的尺寸，h为垄间农作物种植地面与垄间行驶地面高度差；z_leftzmax,z_rightzmax分别是点Point_leftzmax、Point_rightzmax的z轴坐标值；

C)选取集合Cloud_left和Cloud_right中满足以下两个条件的点云，分别对应存入集合Cloud_leftcrops和Cloud_rightcrops中，并按升序排列：与平面S_planey:y＝0.2×iteration_cloud的距离distance_planey≤K1、与平面S_planezcrops:z＝h_crops的距离distance_planezcrops≤K1；

D)分别在集合Cloud_leftcrops和Cloud_rightcrops中找到x值最接近于0的点云数据，分别记作Point_leftcrops和Point_rightcrops，Point_leftcrops和Point_rightcrops坐标分别为(x_{cloudleftcrops},y_{cloudleftcrops},z_{cloudleftcrops})和(x_{cloudrightcrops},y_{cloudrightcrops},z_{cloudrightcrops})；

E)计算避碰参考点Point_{avoidancereference}(x_{avoidancereference}，y_{avoidancereference}，z_{avoidancereference})，并存入Reference_{avoidancecrops}中；

F)选取集合Reference_{temporaryridge}和Reference_{avoidancecrops}中y值最接近的两个参考点，计算最佳行驶参考点并存入集合Reference_bestdrive：

考虑到棚内农作物如果生成茂盛可能会导致机器人在垄间行驶过程中发生触碰，对农作物造成损害，生成的垄间避碰参考点可以避免机器人在行驶过程中触碰到农作物；同时为了保证机器人在行驶过程中不碰到农作物的同时不会因为垄间凹形地面而发生侧翻，因此结合垄间行驶地面参考点和垄间行驶避碰参考点生成最佳行驶参考点。

步骤S5的具体实现过程包括：

1)选取航向角为0、俯仰角为0的距离为机器人距离垄间尽头墙面的距离distance_ridgewall；

2)判断是否满足(distance_ridgewall≤distance_{ridgewallthreshold})||(|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot)或(|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot)||(y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot)，其中distance_{ridgewallthreshold}＝1.5×length_robot，length_robot为机器人在机器人坐标系y轴方向上的长度，y_{avoidancereference,j}、y_{cloudleftcrops,j}、y_{cloudrightcrops,j}分别为集合Reference_{avoidancecrops}、Cloud_leftcrops、Cloud_rightcrops中第j个避碰参考点的纵坐标；若满足distance_ridgewall≤distance_{ridgewallthreshold}，则表示机器人已抵达垄间道路尽头，转到步骤3)；若满足|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot，则说明机器人即将抵达垄间道路尽头，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0y_turn＝0.5×(y_{avoidancereference,j+1}+y_{avoidancereference,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若满足|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot，则说明机器人即将向左转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，y_turn＝0.5×(y_{cloudleftcrops,j+1}+y_{cloudleftcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若满足|y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot，则说明机器人即将向右转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，y_turn＝0.5×(y_{cloudrightcrops,j+1}+y_{cloudrightcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若不满足，则机器人进行直线行驶；||表示或运算符；

3)若转向标志位Mode_turn＝0，则机器人右转90°，更新Mode_turn＝1；

若Mode_turn＝1，则机器人右转90°，更新Mode_turn＝2；

若Mode_turn＝2且直行标志位flag_straight不存在，则机器人左转90°，更新

Mode_turn＝3；

若Mode_turn＝2且flag_straight＝1，说明此时农药喷洒任务进度为50％，机器人直行，驶入对侧垄间，更新Mode_turn＝0；

若Mode_turn＝2且flag_straight＝2，说明此时农药喷洒任务进度为100％，则机器人根据最佳行驶参考点行驶至棚内主路，随后切换至目标导航模式，更新Mode＝1，转到步骤7)中；

若Mode_turn＝3，则机器人左转90°，更新Mode_turn＝0；

若不存在转向标志位Mode_turn，则从激光雷达探测到的距离数组中，分别选取航向角为-90°和90°、俯仰角为0的距离为机器人距离侧边墙面的距离distance_{ridgewallleftside}、distance_{ridgewallrighttside}，若distance_{ridgewallleftside}≤3×width_ridge，其中width_ridge为垄间道路宽度，则机器人右转90°，同时记Mode_turn＝3；若distance_{ridgewallrightside}≤3×width_ridge，则机器人左转90°，同时记Mode_turn＝1；

4)若Mode_turn＝1，判断是否满足distance_ridgewall≤2×width_ridge与是否存在直行标志位flag_straight；若满足distance_ridgewall≤2×width_ridge且不存在flag_straight，则说明此时为棚内左侧最后一条垄间道路，记flag_straight＝1，进行航向角修正；若满足distance_ridgewall≤2×width_ridge且存在flag_straight＝1，则说明此时为棚内右侧最后一条垄间道路，更新flag_straight加1，进行航向角修正；若不满足distance_ridgewall≤2×width_ridge，则生成转向后垄间行驶地面参考点，记iteration_cloud＝0，同时生成行驶参考点Point_{temporaryreference}并存入集合Reference_ridgeturn；

5)若Mode_turn＝0，则说明此时机器人进入垄间道路，则生成最佳行驶参考点进行垄间直线行驶；

6)若Mode_turn＝2，则说明此时机器人即将进入垄间道路，同时判断是否满足distance_{ridgewallleftside}≤2×width_ridge与是否存在直行标志位flag_straight；若满足distance_{ridgewallleftside}≤2×width_ridge且不存在flag_straight，则说明此时为棚内左侧最后一条垄间道路，记flag_straight＝1；若满足distance_{ridgewallleftside}≤2×width_ridge且存在flag_straight＝1，则更新flag_straight加1，生成最佳行驶参考点；若不满足distance_{ridgewallleftside}≤2×width_ridge，生成最佳行驶参考点；

7)机器人切换至目标导航模式，选取仓库坐标G_warehouse作为目标点，生成基于地图车道线的全局路径，返回仓库，农药喷洒任务结束。

1)当机器人抵达垄间道路尽头时，还进行航向角修正，具体过程包括：

若不存在转向标志位Mode_turn或Mode_turn＝0或Mode_turn＝2，则判断是否满足distance_ridgewall≤3×length_robot；若满足，则机器人抵达垄间道路尽头后，从机器人的激光雷达探测到的距离数组中，从航向角为-10°至10°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠0，则进行机器人航向角修正，直至Yaw_standard＝0；若不满足，则规定此时的Yaw_standard＝0；

若Mode_turn＝1，则判断是否满足distance_{ridgewallleftside}≤1.5×length_robot，其中distance_{ridgewallleftside}为激光雷达探测的航向角为-90°，俯仰角为0的距离；若满足，则从激光雷达探测到的距离数组中，从航向角为-100°至-80°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠-90°，则进行机器人航向角修正，直至Yaw_standard＝-90°；若不满足，则规定此时的Yaw_standard＝-90°；若Mode_turn＝3，则判断是否满足distance_{ridgewallrightside}≤1.5×length_robot，其中distance_{ridgewallleftside}为激光雷达探测的航向角为90°，俯仰角为0的距离；若满足，则从激光雷达探测到的距离数组中，从航向角为80°至100°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠90°，则进行机器人航向角修正，直至Yaw_standard＝90°；若不满足，则规定此时的航向角Yaw_standard＝90°。

由于垄间道路呈直角S形，通过激光雷达传感器信息实现机器人自主判断是否需要转弯以及是否为棚内左侧或棚内右侧最后一条垄间，实现机器人在棚内完全自主化作业。

本发明还提供了一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；所述处理器执行所述计算机程序，以实现本发明所述方法的步骤。

本发明还提供了一种计算机程序产品，包括计算机程序/指令；其特征在于，该计算机程序/指令被处理器执行时实现本发明所述方法的步骤。

与现有技术相比，本发明所具有的有益效果为：本发明首先根据机器人定位信息选取与机器人距离最近的车道线，并通过机器人定位信息和最近车道线信息生成引导路径参考点，将机器人引导至最近车道线上；其次，机器人生成基于高精地图车道线的全局路径，待机器人抵达棚内主路后，自适应切换至任务导航模式，开始农药喷洒任务；通过机载激光雷达采集到的环境信息生成垄间行驶地面参考点以及避碰参考点，地面参考点可以保证机器人在垄间行驶时不会因为垄间地形的原因而发生侧翻，而避碰参考点可以保证机器人在垄间主路行驶时避开生长茂盛的农作物以免造成损害，通过对这两个参考点进行加权处理，生成最佳行驶参考点保证了机器人在垄间行驶的过程中在不会发生侧翻的同时，能够避开生长过于茂盛的农作物；同时通过激光雷达采集到的航向角以及距离信息实现对垄间直角弯道判断的功能以及农药喷洒任务完成进度的监控，当机器人判断农药喷洒任务完成后，自适应切换回目标导航模式，生成一条基于高精地图车道线的全局路径回到仓库，等待下一次农药喷洒任务点发布，智能化程度更高。

附图说明

图1为机器人导航规划流程图；

图2为本发明的车道线选取示意图；

图3为本发明的引导路径参考点生成示意图；

图4为本发明的车道线及其引导路径仿真图；

图5为本发明引导路径距离车道线侧向距离差值图；

图6为本发明的智慧农场二维平面图；

图7为本发明的垄间行驶路径示意图；

图8为本发明的垄间道路主视图；

图9为本发明的棚内主路主视图。

具体实施方式

本发明实施例提供的一种面向智慧农场的农药喷洒机器人导航规划方法，如图1所示。包括以下步骤：

步骤一：机器人从智慧农场仓库出发，根据机器人定位信息判断离机器人当前位置最近的车道线：

步骤1，读取棚内农药喷洒起始点坐标文件，将棚内农药喷洒起始点Point_spraying,a存入农药喷洒点点集G_spraying＝{Point_spraying,a|a＝1、2、...、Num_spraying}中，其中Num_spraying表示农药喷洒点总数；记录仓库坐标G_warehouse；

步骤2，用户选定某一个农药喷洒点Point_spraying，机器人从仓库出发，导航模式为目标导航模式，记为Mode＝1，目标点为农药喷洒点Point_spraying；

步骤3，遍历所有的车道线Lane_i，其中i表示车道线编号；判断在第i条车道线Lane_i是否存在轨迹点Point_ij，其中j表示车道线Lane_i的第j个轨迹点，j＝1,6,…,(5k+1)；k为大于0的正整数；若存在，则转到步骤4；若不存在，则转到步骤5；

步骤4，选取车道线Lane_i的轨迹点Point_ij，并计算轨迹点Point_ij距离机器人当前位置Point_current的距离distance_ij，将距离distance_ij以及对应的车道线编号i分别存入集合Distance_all＝{distance_ij|j＝1、6、…、(5k+1)}和集合Index_all＝{i|i＝1、2、…}中；(x_ij,y_ij)和(x_c,y_c)分别是轨迹点Point_ij和机器人当前位置Point_current的坐标；

步骤5，考虑到可能找到的最近轨迹点位于其所在车道线末端，此时再将机器人引导至该车道线则无意义，因此从集合Distance_all中选取来自两条不同车道线的最短距离distance_min1、distance_min2，并在集合Index_all中对应选取该两条不同车道线的编号i_min1、i_min2；

步骤6，遍历车道线编号为i_min1、i_min2的两条车道线Lane_min1、Lane_min2上的所有轨迹点Point_min,bd，并计算轨迹点Point_min,bd距离机器人当前位置Point_current的距离distance_min,bd，将距离distance_min,bd以及对应的车道线编号i_min分别存入集合Distance_min＝{distance_min,bd|b＝1、2，d＝1、2、…}和集合Index_min＝{b|b＝1、2}中；其中distance_min,bd的计算公式：(x_min,bd,y_min,bd)和(x_c,y_c)分别是轨迹点Point_min,bd和机器人当前位置Point_current的坐标。

步骤二：根据机器人定位信息以及最近车道线信息生成车道线引导路径参考点：

步骤1，定义引导路径参考点数量Num_{guidepathpoint}＝0；

步骤2，在集合Distance_min中查找最短距离并记为distance_temporarymin，根据最小距离distance_temporarymin对应查找distance_temporarymin在车道线Lane_temporarymin上的轨迹点，该轨迹点即为距离机器人当前位置最近的轨迹点Point_{temporarycurrentmin}，同时查找轨迹点Point_{temporarycurrentmin}所在车道线的编号i_temporarymin，该车道线编号对应的车道线即为选取的距离机器人当前位置最近的车道线，如图2所示，找到最短距离distance_temporarymin所对应的车道线，沿着车道线前进方向计算从轨迹点Point_{temporarycurrentmin}至车道线末尾轨迹点Point_{temporarycurrentend}的数量Num_{pointtemporary}；

步骤3，判断是否满足Num_{pointtemporary}≤50；若满足，则再将机器人引导至该车道线上则无意义，因此这时需要引导至选取的两条车道线中另一条车道线上，在集合Index_min＝{b|b＝1、2}中找到i≠i_temporarymin的车道线编号并记为i_min，对应的车道线记为Lane_min，转到步骤4；若不满足，则说明最近轨迹点并不位于其所在车道线末端，i_min＝i_temporarymin、Point_currentmin＝Point_{temporarycurrentmin}、Lane_min＝Lane_temporarymin，转到步骤5；

步骤4，判断是否已找到轨迹点Point_currentmin；若已找到，则转到步骤5；若未找到，则遍历车道线Lane_min上所有的轨迹点Point_j，找到距离机器人当前位置最近的轨迹点Point_currentmin，具体过程如下：

步骤4.1，计算轨迹点Point_j与机器人当前位置Point_current的距离distance_lanemin,j并存入集合Distance_lanemin中，其中距离distance_lanemin,j计算式与距离distance_ij一致；

步骤4.2，在集合Distance_lanemin中选取最短距离distance_currentmin，并相应的在车道线Lane_min上找到最短距离distance_currentmin所对应的轨迹点，该轨迹点即为距离机器人当前位置最近的轨迹点Point_currentmin；

步骤5，判断是否满足Num_{guidepathpoint}≤10；若满足，则distance_temporary＝distance_currentmin、Num_{guidepathpoint}＝Num_{guidepathpoint}+1，转到步骤6；若不满足，则转到步骤5.1；

步骤5.1，计算distance_temporary＝distance_currentmin-0.2×(Num_{guidepathpoint}-10)、Num_{guidepathpoint}＝Num_{guidepathpoint}+1；

步骤5.2，判断是否满足distance_temporary≥0；若满足，则转到步骤6；若不满足，则说明机器人已抵达车道线上，转到步骤三步骤1中；

步骤6，在车道线Lane_min上朝着目标点方向选取点Point_currentmin的下一个轨迹点Point_currentmin+1，获取其位置信息(x_currentmin+1,y_currentmin+1)并计算从Point_currentmin到Point_currentmin+1的方向角α，计算机器人当前位置Point_current至轨迹点Point_currentmin的方向角β；

步骤7，判断方向角α和方向角β之间的角度差是否满足75°≤|α-β|≤105°；若满足，则转到步骤10，并且记Point_current′＝Point_current为距离轨迹点Point_currentmin最近的坐标点，记录插值标志位flag_expand＝0；若不满足，则转到步骤8，记录flag_expand＝1；

步骤10，判断坐标点Point_current′是在车道线Lane_min前行方向的左侧还是右侧，具体过程如下：

步骤10.1，计算车道线Lane_min上的两个轨迹点Point_currentmin和Point_currentmin+1的直线方程：

(y_currentmin+1-y_currentmin)x-(x_currentmin+1-x_currentmin)y+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＝0

其中，(x_currentmin,y_currentmin)和(x_currentmin+1,y_currentmin+1)分别为点Point_currentmin和Point_currentmin+1的坐标；

步骤10.2，将Point_current′的坐标(x_current′,y_current′)代入步骤10.1的公式中，若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＞0，则distance＝distance_temporary；若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＜0，则distance＝-distance_temporary；

步骤11，若插值标志位flag_expand＝1，采用三次样条插值法生成从机器人当前位置Point_current至坐标点Point_current′的参考点，并将参考点坐标存入参考点集合Reference_guidepath中，并转到步骤12；若插值标志位flag_expand＝0，则不需要生成从机器人当前位置Point_current至坐标点Point_current′的参考点，直接转到步骤12；

步骤12，计算引导路径参考点Point_reference并存入参考点集合Reference_guidepath中，具体计算式如下：

其中，(x_currentmin+1,y_currentmin+1)是点Point_currentmin+1的坐标，是常数，(x_reference,y_reference)是参考点Point_reference的坐标。

如图3所示，其中生成的引导路径参考点为图中的空心圆点，车道线轨迹点为实心圆点；图4为仿真环境中的车道线及其引导路径横纵坐标值，图5为仿真环境中计算得到的引导路径参考点至车道线距离值，可以看出，在刚开始的2米内引导路径参考点至车道线距离值不变，然后距离值开始逐渐减小，当距离值减小为0时说明引导路径参考点在车道线上。

步骤三：机器人抵达棚内主路时，装配有激光雷达的机器人在垄间道路自主行驶时，采集棚内作业过程中激光雷达探测到的环境信息，生成机器人垄间行驶地面参考点：

步骤1，机器人抵达至车道线上，根据机器人定位信息和农药喷洒目标点坐标生成基于高精地图车道线的全局路径，机器人根据生成的全局路径抵达农药喷洒目标点，该喷洒点位于棚内主路上，切换至任务导航模式，记为Mode＝2，记获得的点云帧数iteration_cloud＝0；建立机器人坐标系，该坐标系以机器人的质心在地面上的投影为原点，机器人前进方向为y轴正方向，机器人右侧为x轴正方向，上方为z轴正方向；

步骤2，机器人左转90°，激光雷达采集到的点云数据相对机器人的坐标为(x,y,z)；

步骤3，获得一帧点云数据，更新iteration_cloud加1，将点云数据按照y≤0和y＞0进行分类，分别记作Cloud_left和Cloud_right；

步骤4，分别选取集合Cloud_left和Cloud_right中满足以下两个条件的点云并存入集合Cloud_leftdemand和Cloud_rightdemand中：与平面S_planey:y＝0.2×iteration_cloud的距离distance_planey≤K1、与平面S_planez:z＝K2的距离distance_planez≤K1；K1、K2为设定的阈值，均为0.05；

步骤5，分别在集合Cloud_leftdemand和Cloud_rightdemand中找到在机器人坐标系中横坐标值最接近于0的点云数据，分别记作Point_{leftreference}和Point_{rightreference}，Point_{leftreference}和Point_{rightreference}坐标分别为(x_{leftreference},y_{leftreference},z_{leftreference})、(x_{rightreference},y_{rightreference},z_{rightreference})；

步骤6，计算垄间行驶参考点Point_{temporaryreference}并存入集合Reference_{temporaryridge}，具体计算式如下：

步骤7，清空集合Cloud_leftdemand和Cloud_rightdemand，用于在获取下一帧点云数据后的点云数据存储。

步骤四：根据激光雷达在行驶过程中采集到的环境信息，生成垄间行驶避碰参考点，结合垄间行驶避碰参考点和垄间行驶地面参考点生成最佳行驶参考点：

步骤1，在集合Cloud_left中找到z值最大的点云，记为Point_leftzmax；在集合Cloud_right找到z值最大的点云，记为Point_rightzmax；

步骤2，为了保证选取的左右点云高度一致，需要定义一个平面高度h_crops，其中h_crops＝(min{z_leftzmax,z_rightzmax,z_robot}+h)/2，z_robot为机器人在z轴上的尺寸，h为垄间农作物种植地面与垄间行驶地面高度差；z_leftzmax,z_rightzmax分别是点Point_leftzmax、Point_rightzmax的z轴坐标值；

步骤3，选取集合Cloud_left和Cloud_right中满足以下两个条件的点云集，分别对应存入集合Cloud_leftcrops和Cloud_rightcrops中，并按升序排列：与平面S_planey:y＝0.2×iteration_cloud的距离distance_planey≤K1、与平面S_planezcrops:z＝h_crops的距离distance_planezcrops≤K1；K1为设定的阈值0.05；

步骤4，分别在集合Cloud_leftcrops和Cloud_rightcrops中找到x值最接近于0的点云数据，分别记作Point_leftcrops和Point_rightcrops，Point_leftcrops和Point_rightcrops坐标分别为(x_{cloudleftcrops},y_{cloudleftcrops},z_{cloudleftcrops})和(x_{cloudrightcrops},y_{cloudrightcrops},z_{cloudrightcrops})；

步骤5，计算避碰参考点Point_{avoidancereference}并存入Reference_{avoidancecrops}中，具体计算式如下：

步骤6，选取集合Reference_{temporaryridge}和Reference_{avoidancecrops}中y值最接近的两个参考点，计算最佳行驶参考点并存入集合Reference_bestdrive：

步骤五：根据激光雷达在行驶过程中采集到的环境信息获取机器人距离垄间尽头墙面的距离，进行垄间直角弯道的判断，同时进行农药喷洒任务完成度的判断：

步骤1，从激光雷达探测到的距离数组中，选取航向角为0、俯仰角为0的距离为机器人距离垄间尽头墙面的距离distance_ridgewall；

步骤2，判断是否满足

(distance_ridgewall≤distance_{ridgewallthreshold})||(|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot)或(|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot)||(y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot)，其中distance_{ridgewallthreshold}＝1.5×length_robot，length_robot为机器人在机器人坐标系y轴方向上的长度，y_{avoidancereference,j}、y_{cloudleftcrops,j}、y_{cloudrightcrops,j}分别为集合Reference_{avoidancecrops}、Cloud_leftcrops、Cloud_rightcrops中第j个避碰参考点的纵坐标；若满足distance_ridgewall≤distance_{ridgewallthreshold}，则表示机器人已抵达垄间道路尽头，转到步骤3；若满足|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot，则说明机器人即将抵达垄间道路尽头，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0

y_turn＝0.5×(y_{avoidancereference,j+1}+y_{avoidancereference,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3；若满足|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot，则说明机器人即将向左转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，

y_turn＝0.5×(y_{cloudleftcrops,j+1}+y_{cloudleftcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3；若满足|y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot，则说明机器人即将向右转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，

y_turn＝0.5×(y_{cloudrightcrops,j+1}+y_{cloudrightcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3；若不满足，则机器人进行直线行驶；||表示或运算符；

步骤3，机器人进行转向，具体过程如下：

若转向标志位Mode_turn＝0，则机器人右转90°，更新Mode_turn＝1；

若Mode_turn＝1，则机器人右转90°，更新Mode_turn＝2；

若Mode_turn＝2且直行标志位flag_straight不存在，则机器人左转90°，更新Mode_turn＝3；

若Mode_turn＝2且flag_straight＝2，说明此时农药喷洒任务进度为100％，则机器人根据最佳行驶参考点行驶至棚内主路，随后切换至目标导航模式，更新Mode＝1，转到步骤8中；

若Mode_turn＝3，则机器人左转90°，更新Mode_turn＝0；

步骤4，若Mode_turn＝1，判断是否满足distance_ridgewall≤2×width_ridge与是否存在直行标志位flag_straight；若满足distance_ridgewall≤2×width_ridge且不存在flag_straight，则说明此时为棚内左侧最后一条垄间道路，记flag_straight＝1，进行航向角修正；若满足distance_ridgewall≤2×width_ridge且存在flag_straight＝1，则说明此时为棚内右侧最后一条垄间道路，更新flag_straight加1，进行航向角修正；若不满足distance_ridgewall≤2×width_ridge，则生成转向后垄间行驶地面参考点，记iteration_cloud＝0，同时生成行驶参考点Point_{temporaryreference}并存入集合Reference_ridgeturn；

步骤5，进行航向角修正，具体过程如下：

若Mode_turn＝1，则判断是否满足distance_{ridgewallleftside}≤1.5×length_robot，其中distance_{ridgewallleftside}为激光雷达探测的航向角为-90°，俯仰角为0的距离；若满足，则从激光雷达探测到的距离数组中，从航向角为-100°至-80°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠-90°，则进行机器人航向角修正，直至Yaw_standard＝-90°；若不满足，则规定此时的Yaw_standard＝-90°；

若Mode_turn＝3，则判断是否满足distance_{ridgewallrightside}≤1.5×length_robot，其中distance_{ridgewallleftside}为激光雷达探测的航向角为90°，俯仰角为0的距离；若满足，则从激光雷达探测到的距离数组中，从航向角为80°至100°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠90°，则进行机器人航向角修正，直至Yaw_standard＝90°；若不满足，则规定此时的Yaw_standard＝90°。

步骤6，若Mode_turn＝0，则说明此时机器人进入垄间道路，则生成最佳行驶参考点进行垄间直线行驶；

步骤7，若Mode_turn＝2，则说明此时机器人即将进入垄间道路，同时判断是否满足distance_{ridgewallleftside}≤2×width_ridge与是否存在直行标志位flag_straight；若满足distance_{ridgewallleftside}≤2×width_ridge且不存在flag_straight，则说明此时为棚内左侧最后一条垄间道路，记flag_straight＝1；若满足distance_{ridgewallleftside}≤2×width_ridge且存在flag_straight＝1，则更新flag_straight加1，生成最佳行驶参考点；若不满足distance_{ridgewallleftside}≤2×width_ridge，生成最佳行驶参考点；

机器人行驶示意图如图7中箭头所示，其中机器人从棚外进入棚内主路，探测到第一条垄间道路时左转，进入垄间道路行驶，当行驶至道路尽头后满足设定条件开始右转90°，继续行驶，当在行驶过程中探测到右侧有第二条垄间道路时开始右转90°，进入第二条垄间道路作业；当机器人沿着第二条垄间道路行驶至棚内主路后，左转90°，沿着棚内主路行驶，直至探测到第三条垄间道路，左转进入垄间道路作业，以此类推，直至行驶至棚内左侧最后一条垄间道路后，沿着最后一条垄间道路行驶至棚内主路后不转向，直线行驶至对侧垄间道路中继续作业。

其中垄间道路与两侧农作物间存在一定的高度差，如图8所示，同时棚内主路与两侧垄间道路也存在一定的高度差，如图9所示。

步骤8，机器人切换至目标导航模式，选取仓库坐标G_warehouse作为目标点，生成基于高精地图车道线的全局路径，返回仓库，农药喷洒任务结束。

本发明提出了一种面向智慧农场的农药喷洒机器人导航规划方法。该方法首先根据机器人定位信息选取与机器人距离最近的车道线，并通过机器人定位信息和最近车道线信息生成引导路径参考点，将机器人引导至最近车道线上；其次，机器人生成基于高精地图车道线的全局路径，待机器人抵达棚内主路后，自适应切换至任务导航模式，开始农作物农药喷洒任务；通过机载激光雷达采集到的环境信息生成最佳行驶参考点，该参考点可以很好的保证机器人在垄间主路行驶不发生侧翻的同时不会碰撞到农作物以免对农作物造成损害；同时通过激光雷达采集到的航向角以及距离信息实现对垄间直角弯道判断的功能以及农药喷洒任务完成进度的监控，当机器人判断农药喷洒任务完成后，自适应切换至目标导航模式，生成一条基于高精地图车道线的全局路径回到仓库，等待下一次农药喷洒任务点的发布。

Claims

1.一种农药喷洒机器人导航规划方法，其特征在于，包括以下步骤：

S5、机器人根据最佳行驶参考点行驶时，根据环境信息获取机器人距离垄间尽头墙面的距离，进行垄间直角弯道的判断，完成农药喷洒任务完成度的判断；

步骤S1的具体实现过程包括：

1)读取棚内农药喷洒起始点坐标文件，将棚内农药喷洒起始点Point_spraying,a存入农药喷洒点点集G_spraying＝{Point_spraying,a|a＝1、2、…、Num_spraying}中，其中Num_spraying表示农药喷洒点总数；

6)遍历车道线编号为i_min1、i_min2的两条车道线Lane_min1、Lane_min2上的所有轨迹点Point_min,bd，并计算轨迹点Point_min,bd距离机器人当前位置Point_current的距离distance_min,bd，将距离distance_min,bd以及对应的车道线编号i_min分别存入集合Distance_min＝{distance_min,bd|b＝1、2，d＝1、2、…、num_b}和集合Index_min＝{b|b＝1、2}中，其中num_b表示编号为b的车道线轨迹点数量；distance_min,bd的计算公式：(x_min,bd,y_min,bd)和(x_c,y_c)分别是轨迹点Point_min,bd和机器人当前位置Point_current的坐标；

步骤S2的具体实现过程包括：

步骤1，定义引导路径参考点数量Num_{guidepathpoint}＝0；

步骤4，判断是否已找到轨迹点Point_currentmin，若已找到，则转到步骤6；若未找到，则遍历车道线Lane_min上所有的轨迹点Point_j，找到距离机器人当前位置最近的轨迹点Point_currentmin，转到步骤5；

步骤5，判断是否满足Num_{guidepathpoint}≤10；若满足，则distance_temporary＝distance_currentmin、引导路径参考点数量Num_{guidepathpoint}加1，转到步骤6；若不满足，则计算distance_temporary＝distance_currentmin-0.2×(Num_{guidepathpoint}-10)、引导路径参考点数量Num_{guidepathpoint}加1，判断是否满足distance_temporary≥0；若满足，则转到步骤6；若不满足，则说明机器人已抵达车道线；

步骤10，若插值标志位flag_expand＝1，则采用三次样条插值法从机器人当前位置Point_current至点Point_current′进行插值，并将插值点坐标存入参考点集合Reference_guidepath中，转到步骤11；若插值标志位flag_expand＝0，则Point_current′＝Point_current，转到步骤11；

其中，(x_currentmin+1,y_currentmin+1)是点Point_currentmin+1的坐标，(x_reference,y_reference)是参考点Point_reference的坐标；distance为轨迹点Point_current′与车道线之间的距离；

步骤S3的具体实现过程包括：

G)清空集合Cloud_leftdemand和Cloud_rightdemand；

步骤S4的具体实现过程包括：

H)在集合Cloud_left中找到z值最大的点云，记为Point_leftzmax；在集合Cloud_right找到z值最大的点云，记为Point_rightzmax；

I)选取平面高度h_crops，其中h_crops＝(min{z_leftzmax,z_rightzmax,z_robot}+h)/2，z_robot为机器人在z轴上的尺寸，h为垄间农作物种植地面与垄间行驶地面高度差；z_leftzmax,z_rightzmax分别是点Point_leftzmax、Point_rightzmax的z轴坐标值；

J)选取集合Cloud_left和Cloud_right中满足以下两个条件的点云集，分别对应存入集合Cloud_leftcrops和Cloud_rightcrops中，并按升序排列：与平面S_planey:y＝0.2×iteration_cloud的距离distance_planey≤K1、与平面S_planezcrops:z＝h_crops的距离distance_planezcrops≤K1；

K)分别在集合Cloud_leftcrops和Cloud_rightcrops中找到x值最接近于0的点云数据，分别记作Point_leftcrops和Point_rightcrops，Point_leftcrops和Point_rightcrops坐标分别为(x_{cloudleftcrops},y_{cloudleftcrops},z_{cloudleftcrops})和(x_{cloudrightcrops},y_{cloudrightcrops},z_{cloudrightcrops})；

L)计算避碰参考点Point_{avoidancereference}(x_{avoidancereference}，y_{avoidancereference}，z_{avoidancereference})，并存入Reference_{avoidancecrops}中；

M)选取集合Reference_{temporaryridge}和Reference_{avoidancecrops}中y值最接近的两个参考点，计算最佳行驶参考点并存入集合Reference_bestdrive：

步骤S5的具体实现过程包括：

2)判断是否满足(distance_ridgewall≤distance_{ridgewallthreshold})||(|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot)或(|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot)||(y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot)，其中distance_{ridgewallthreshold}＝1.5×length_robot，length_robot为机器人在机器人坐标系y轴方向上的长度，y_{avoidancereference,j}、y_{cloudleftcrops,j}、y_{cloudrightcrops,j}分别为集合Reference_{avoidancecrops}、Cloud_leftcrops、Cloud_rightcrops中第j个避碰参考点的纵坐标；若满足distance_ridgewall≤distance_{ridgewallthreshold}，则表示机器人已抵达垄间道路尽头，转到步骤3)；若满足|y_{avoidancereference,j+1}-y_{avoidancereference,j}|≥length_robot，则说明机器人即将抵达垄间道路尽头，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0y_turn＝0.5×(y_{avoidancereference,j+1}+y_{avoidancereference,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若满足|y_{cloudleftcrops,j+1}-y_{cloudleftcrops,j}|≥length_robot，则说明机器人即将向左转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，y_turn＝0.5×(y_{cloudleftcrops,j+1}+y_{cloudleftcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若满足|y_{cloudrightcrops,j+1}-y_{cloudrightcrops,j}|≥length_robot，则说明机器人即将向右转向垄间道路，计算垄间转向点Point_turn(x_turn,y_turn,z_turn)，其中x_turn＝0，y_turn＝0.5×(y_{cloudrightcrops,j+1}+y_{cloudrightcrops,j})，z_turn＝0，机器人行驶至转向点后，转到步骤3)；若不满足，则机器人直线行驶；||表示或运算符；

若Mode_turn＝1，则机器人右转90°，更新Mode_turn＝2；

若Mode_turn＝3，则机器人左转90°，更新Mode_turn＝0；

4)若Mode_turn＝1，判断是否满足distance_ridgewall≤2×width_ridge与是否存在flag_straight；若满足distance_ridgewall≤2×width_ridge且不存在flag_straight，则说明此时为棚内左侧最后一条垄间道路，记flag_straight＝1，进行航向角修正；若满足distance_ridgewall≤2×width_ridge且存在flag_straight＝1，则说明此时为棚内右侧最后一条垄间道路，更新flag_straight加1，进行航向角修正；若不满足distance_ridgewall≤2×width_ridge，则生成转向后垄间行驶地面参考点，记iteration_cloud＝0，同时生成行驶参考点Point_{temporaryreference}并存入集合Reference_ridgeturn；

2.根据权利要求1所述的农药喷洒机器人导航规划方法，其特征在于，步骤11中，distance的确定过程包括：

计算车道线Lane_min上的两个轨迹点Point_currentmin和Point_currentmin+1的直线方程：

(y_currentmin+1-y_currentmin)x-(x_currentmin+1-x_currentmin)y+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＝0；

将轨迹点Point_current′的坐标代入直线方程中进行判断，若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＞0则说明Point_current′在车道线的右侧，distance＝distance_temporary；若(y_currentmin+1-y_currentmin)x_current′-(x_currentmin+1-x_currentmin)y_current′+(x_currentmin+1·y_currentmin-y_currentmin+1·x_currentmin)＜0则说明Point_current′在车道线的左侧，distance＝-distance_temporary。

3.根据权利要求1所述的农药喷洒机器人导航规划方法，其特征在于，当机器人抵达垄间道路尽头时，还进行航向角修正，具体过程包括：

若Mode_turn＝3，则判断是否满足distance_{ridgewallrightside}≤1.5×length_robot，其中distance_{ridgewallleftside}为激光雷达探测的航向角为90°，俯仰角为0的距离；若满足，则从激光雷达探测到的距离数组中，从航向角为80°至100°、俯仰角为0的探测数据中选取距离值最小的一组激光数据作为标准距离，记为distance_standard，其航向角记为Yaw_standard，若Yaw_standard≠90°，则进行机器人航向角修正，直至Yaw_standard＝90°；若不满足，则规定此时的航向角Yaw_standard＝90°。

4.一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；其特征在于，所述处理器执行所述计算机程序，以实现权利要求1～3之一所述方法的步骤。