面向高位大尺寸水果的采运协同机器人控制系统及方法与流程

摘要: 本发明涉及机器人控制系统,具体而言,涉及一种面向高位大尺寸水果的采运协同机器人控制系统及方法。背景技术:1、现有农业机器人在高位大尺寸水果种植区作业时,视觉、激...
面向高位大尺寸水果的采运协同机器人控制系统及方法与流程

本发明涉及机器人控制系统,具体而言,涉及一种面向高位大尺寸水果的采运协同机器人控制系统及方法。

背景技术:

1、现有农业机器人在高位大尺寸水果种植区作业时,视觉、激光、惯性、力或转矩、电流、温湿度以及作物或土壤相关等多源传感数据往往来自不同传感器和不同时刻,缺乏统一的时间基准和坐标基准,导致同一作业对象在感知、规划和执行环节中得到的状态信息不一致,难以形成稳定、可靠的状态表达。由于感知信息时空不统一,后续路径规划和末端操作规划容易偏离实际作业条件,在冠层遮挡严重、目标可见性差的情况下,机器人难以主动调整观察位置或观察角度,无法有效改善目标可见性和作业可达性,影响目标接近、承托、分离和缓降收纳等动作的准确性。

2、现有农业机器人通常也缺乏统一的安全约束比较机制,作业过程中碰撞、倾覆、滑移、末端接触力异常、人员接近和能量不足等风险难以被持续监测并统一处理,安全边界不明确,容易出现减速不及时、暂停不果断、撤回或接管机制不完善等问题。同时,感知、规划、控制与执行之间缺少有效衔接,底盘、机械臂和末端执行器往往各自独立动作,难以根据统一的状态信息、安全约束和作业规划协同执行,导致高位大尺寸水果采收过程中整机稳定性差、动作协调性不足、执行结果难以反馈至后续控制。

3、在坡地等非结构化地形上,现有农业机器人进行大臂展重载作业时,底盘稳定控制通常仅依据当前姿态或单一传感器信息,缺乏对历史执行状态、安全边界和作业任务的综合考量,难以对姿态、支腿或配重进行统一调整,重载作业中容易发生失稳。高刚度大臂展机械臂和重载模块化末端执行器在执行目标接近、承托、分离和缓降收纳动作时,也缺少与底盘稳定状态、观察视角和作业任务的协同,难以将执行结果反馈至后续控制,无法形成稳定的作业闭环。

4、此外,现有采收设备与转运设备之间缺少有效的动态任务分配机制,任务安排往往依赖人工经验或静态规则,难以根据作业现场实时变化进行动态调整,导致采收与转运衔接效率低,难以满足多设备连续采运协同作业需求。

5、上述问题使得现有农业机器人在高位大尺寸水果作业场景中难以同时兼顾感知一致性、规划合理性、整机稳定性、作业安全性和采运协同效率。

技术实现思路

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、其中,为第i个任务的所述采收时间,为第i个任务的所述转运等待时间,为能耗权重系数,为第i个任务的所述能耗,为负载均衡惩罚系数,为第j台转运设备的利用率;

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、在分离确认后,所述夹持模块或所述环抱模块释放果实,所述缓降收纳模块根据所述果实重量估计数据和所述果实易损性数据生成缓降速度数据,并根据所述收纳容器压力数据生成冲击力限制数据,根据所述缓降速度数据和所述冲击力限制数据,得到所述末端执行器反馈数据;

54、所述高刚度大臂展机械臂在承托、剪切和缓降过程中根据关节力矩数据和末端负载数据执行位姿补偿和扰动抑制,并输出所述机械臂反馈数据。

55、第二方面,本发明提供一种面向高位大尺寸水果的采运协同机器人控制方法,应用于如第一方面所述的面向高位大尺寸水果的采运协同机器人控制系统,所述面向高位大尺寸水果的采运协同机器人控制方法包括:

56、采集高位大尺寸水果作业场景的多源传感数据;

57、通过统一时间同步总线对所述多源传感数据进行时间同步和坐标统一,得到多层状态表达数据;

58、将所述多层状态表达数据与预设安全约束进行比较,得到安全约束数据;

59、根据所述多层状态表达数据和所述安全约束数据,生成作业规划数据;所述作业规划数据包括路径规划数据和末端操作规划数据;

60、根据所述多层状态表达数据生成视角调整数据;

61、根据所述多层状态表达数据、所述安全约束数据、上一控制周期的执行反馈数据和所述路径规划数据调整姿态、支腿或配重,得到所述底盘稳定数据;

62、根据所述末端操作规划数据、所述视角调整数据和底盘稳定数据执行目标接近、承托、分离和缓降收纳动作,并输出当前控制周期的执行反馈数据;

63、根据所述当前控制周期的执行反馈数据、果实重量数据、成熟度数据、路径距离数据和转运容量数据,生成采收与转运的任务分配数据;所述任务分配数据用于所述坡地稳定底盘和所述高刚度大臂展机械臂的任务执行。

64、本发明的面向高位大尺寸水果的采运协同机器人控制系统及方法的有益效果是:

65、通过主动视角感知装置采集多源传感数据,并由状态估计单元进行时间同步和坐标统一,形成统一的多层状态表达数据,使高位大尺寸水果作业场景中的地形、作物、机体和环境状态能够在同一时空基准下被感知和利用;安全约束单元将多层状态表达数据与预设安全约束比较得到安全约束数据,使后续规划和执行始终受到安全边界约束;智能控制器据此生成包括路径规划数据和末端操作规划数据的作业规划数据,主动视角感知装置根据多层状态表达数据生成视角调整数据,从而在冠层遮挡条件下改善目标可见性和作业可达性;坡地稳定底盘根据多层状态表达数据、安全约束数据、上一控制周期的执行反馈数据和路径规划数据调整姿态、支腿或配重,得到底盘稳定数据,使大臂展重载作业在坡地等非结构化地形上仍能保持整机稳定;高刚度大臂展机械臂和重载模块化末端执行器根据末端操作规划数据、视角调整数据和底盘稳定数据执行目标接近、承托、分离和缓降收纳动作,并输出当前控制周期的执行反馈数据,从而实现对高位大尺寸水果的稳定、安全、低损采收;

66、采运协同调度平台根据当前控制周期的执行反馈数据、果实重量数据、成熟度数据、路径距离数据和转运容量数据生成采收与转运的任务分配数据,并用于坡地稳定底盘和高刚度大臂展机械臂的任务执行,从而提高采收与转运之间的衔接效率和多设备连续作业能力;同时,上一控制周期执行反馈数据参与底盘稳定数据生成、当前控制周期执行反馈数据又参与任务分配,使感知、规划、稳定控制、执行和采运调度形成闭环,提升复杂果园环境下自主作业的稳定性、安全性、连续性和可维护性。