一种面向丘陵无序果园的采摘-运输机器人协同动态路径规划系统及方法

本发明属于农业机器人，具体涉及一种面向丘陵无序果园的采摘车-运输车协同动态路径规划系统及方法，尤其适用于芒果、柑橘等种植于丘陵地带、果树呈不规则散布的果园采运协同作业场景。

背景技术：

1、丘陵地区果园的果树常呈不规则散布，果园内道路狭窄、坡度起伏大、转弯半径小，且路面状态随天气变化剧烈。以芒果园为例，芒果树冠幅大、挂果分布不均、单株产量差异显著，运输车满载后果实质量大、重心高，对行驶路径的坡度、侧倾等安全性指标极为敏感。现有采运协同路径规划方案主要存在以下不足：

2、（1）现有方案多基于平原成行果园或梯田分层果园设计，无法适应丘陵无序果园的散布特征。平原成行果园的路径规划依赖于果树成行排列的规则结构，梯田分层果园则利用海拔分层进行串行规划。丘陵无序果园中果树无固定行距、株距，可通行区域受坡度、沟坎、树冠遮挡、临时堆放物和湿滑土壤影响，上述规则化方法难以适用。

3、（2）运输车容量配置与果树产量分布脱节，满载返程浪费严重。现有方案通常假定每棵果树产量均匀或采用固定采摘顺序，未将单株果树产量差异纳入分簇和路径规划。丘陵芒果园中，不同植株因光照、树龄、品种等因素产量差异可达数倍，若按固定顺序采摘，运输车常出现“未装满即需返程”或“超额装载被迫分次运输”的情况，导致空驶里程和能耗显著增加。

4、（3）卸货点位置固定，无法随作业进度动态优化。现有方案通常将卸货点（无人机吊运点或地面卸货点）设于果园边缘固定位置，随采摘推进，运输车满载返程距离逐渐增加，能耗和效率持续恶化。

技术实现思路

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、更进一步地，所述最大安全爬坡角度和最大安全侧倾角度根据地面条件进行确定：对于松软土壤或湿滑路面，将其分别设定为15°和10°；对于硬化路面或干燥硬土，将其分别设定为20°和15°。

28、一种面向丘陵无序果园的采摘-运输机器人协同动态路径规划系统，包括：

29、果园语义地图模块，存储有地形高程与可通行区域、果树点位与估计产量、候选卸货点；

30、在线规划模块，包括丘陵作业图构建模块、坡度危险边排除模块、产量匹配分簇模块、卸货点动态规划模块、协同顺序规划模块、运输车动态接驳模块，丘陵作业图构建模块用于基于车辆通过性约束构建带有通行代价的图结构，坡度危险边排除模块用于根据安全角度阈值直接禁用超限的危险路段，产量匹配分簇模块用于将果树按图距离和载重上限划分为总产量合适的作业簇，卸货点动态规划模块用于通过加权最短路径模型随作业进度动态优化卸货点位置，协同顺序规划模块用于规划簇内采摘最优遍历顺序及簇间切换顺序，运输车动态接驳模块用于根据实时载重状态智能决策并调度空闲车辆接替满载车辆；

31、执行与反馈模块，用于执行系统下发的作业指令并实时上传状态。

32、相较于现有技术，本发明具备以下显著优势：

33、（1）产量匹配分簇，避免半载浪费。通过将果树估计产量与运输车载重上限匹配分簇，每个作业簇的总产量接近载重整数倍，最大限度减少运输车未满载返程。仿真结果表明，相比固定顺序采摘，空驶里程降低40%~60%。

34、（2）动态卸货点优化，降低返程能耗。通过加权最短路径模型动态优化卸货点位置，权重取簇产量，使高产簇靠近卸货点，并随作业进度排除已完成簇。相比固定边界卸货点，满载返程总里程降低15%~25%。

35、（3）坡度危险边硬排除，保障坡地行驶安全。将超限坡度边直接禁用，而非仅加权惩罚，彻底杜绝运输车进入危险路段。针对履带底盘运输车设定20°爬坡和15°侧倾安全阈值，松软湿滑路面自适应降低，从路径规划层面杜绝侧翻和溜坡风险。

36、（4）协同顺序规划兼顾效率与载重状态。簇内以距离+坡度能耗最小化遍历，簇间根据载重状态智能决策-未满载则就近切换，满载则自动换车返仓，实现采运无缝衔接。