基于STM32的清淤系统控制方法及系统与流程

摘要: 本发明属于自动化控制,具体来说,特别涉及基于stm32的清淤系统控制方法及系统。背景技术:1、随着内河航道治理、城市湖泊生态修复及水库除险加固工程的不断推进,水...
基于STM32的清淤系统控制方法及系统与流程

本发明属于自动化控制,具体来说,特别涉及基于stm32的清淤系统控制方法及系统。

背景技术:

1、随着内河航道治理、城市湖泊生态修复及水库除险加固工程的不断推进,水下清淤作业的重要性日益凸显。考虑到复杂水域中大型挖泥船无法进入,小型履带式清淤机器人凭借其体积小、机动性强等优势,成为浅水区及受限水域清淤的主力装备。清淤机器人通常搭载螺旋绞刀进行底泥切削,并通过泥浆泵将固液混合物抽吸排至岸基。

2、现有的清淤机器人控制系统大多依赖人工远程遥控或采用简单的固定参数自动化控制。在实际作业中,操作员通常预先设定固定的履带行进速度与固定的绞刀旋转速度。部分设备虽然安装了电流传感器以监测电机状态,但仅限于提供超过绝对阈值后的切断电源保护功能。系统缺乏对泥浆泵出口浓度、绞刀切削阻力以及车体姿态等多源数据的深度融合分析,且控制回路彼此独立,未形成协同联动的闭环调控机制。

3、由此可以看出,现有技术存在以下问题:1.固定参数作业无法适应底泥密度的非线性突变,底泥较稀时易导致吸入大量清水,排泥浓度低且能耗浪费;2.当绞刀遭遇极硬障碍物或被废弃渔网瞬间缠绕时,简单的过流保护往往响应滞后,导致电机烧毁或必须依赖人工下水打捞脱困;3.在起伏不平的水下地形中,缺乏姿态补偿机制会导致绞刀频繁悬空或过度扎泥,严重影响清淤效率与作业连续性。

技术实现思路

1、(一)解决的技术问题

2、针对相关技术中的问题,本发明提供基于stm32的清淤系统控制方法及系统,以克服现有相关技术所存在的上述技术问题。

3、(二)技术方案

4、为解决上述技术问题,本发明是通过以下技术方案实现的:

5、一方面,本发明提供基于stm32的清淤系统控制方法,包括以下步骤:

6、s1、通过stm32微控制器采集绞刀电机的实时扭矩反馈值、履带电机的实时推力反馈值、泥浆泵出口的实时泥浆浓度值以及六轴姿态传感器的实时俯仰角;

7、s2、计算实时扭矩反馈值的一阶时间导数,得到实时扭矩梯度;

8、判断实时扭矩反馈值是否大于额定扭矩阈值且实时扭矩梯度是否大于突变梯度阈值;

9、若是,控制绞刀电机执行动态脱困序列,直至实时扭矩反馈值下降至安全扭矩阈值以下;否则,执行步骤s3;

10、s3、基于实时泥浆浓度值与目标泥浆浓度的浓度偏差,计算基础绞刀转速调节量与基础履带推进速度调节量;

11、判断实时推力反馈值是否大于安全推力阈值;若是,将基础履带推进速度调节量乘以推力衰减系数得到实际履带推进速度调节量;否则,将基础履带推进速度调节量作为实际履带推进速度调节量;

12、基于额定扭矩阈值、实时扭矩反馈值以及基础绞刀转速调节量,计算实际绞刀转速调节量;将实际绞刀转速调节量下发至绞刀电机,将实际履带推进速度调节量下发至履带电机;

13、s4、将实时俯仰角与水平基准角的姿态偏差输入前馈补偿算法,得到液压推杆补偿行程;利用stm32微控制器控制液压比例阀执行液压推杆补偿行程,调节绞刀大臂的升降高度;

14、本发明通过stm32微控制器同步采集清淤过程中的多模态物理量,利用扭矩一阶时间导数识别突发缠绕工况并执行动态脱困序列,有效避免了电机过载烧毁;在正常工况下,引入模糊比例积分微分控制器计算泥浆浓度偏差,并结合推力反馈与扭矩裕量,将控制量动态分配至绞刀与履带,实现了清淤浓度的闭环稳定与防滑转控制;将姿态传感器获取的俯仰角输入前馈补偿算法,实时调节绞刀大臂高度;在资源受限的情况下实现了多源感知、异常脱困、效能寻优与地形自适应的轻量化协同闭环,提升了清淤系统的自主作业能力与设备安全性。

15、优选地,所述s1包括以下步骤:

16、s11、利用stm32微控制器的控制器局域网总线接口接收绞刀电机驱动器发送的绞刀电流反馈报文与履带电机驱动器发送的履带电流反馈报文;

17、s12、提取绞刀电流反馈报文中的绞刀实时电流值,将绞刀实时电流值乘以绞刀电机扭矩常数,得到实时扭矩反馈值;

18、提取履带电流反馈报文中的履带实时电流值,将履带实时电流值乘以履带电机推力常数,得到实时推力反馈值;

19、s13、利用stm32微控制器的直接内存访问控制器配合模数转换接口,连续读取泥浆浓度传感器的模拟电压信号;利用线性映射函数将模拟电压信号转换为实时泥浆浓度值;

20、s14、利用stm32微控制器的内部集成电路总线接口读取六轴姿态传感器内部寄存器中的四元数数据,利用欧拉角转换公式将四元数数据转换为实时俯仰角;

21、将实时扭矩反馈值、实时推力反馈值、实时泥浆浓度值与实时俯仰角存入滑动窗口缓冲数组中;

22、本发明利用stm32微控制器的控制器局域网总线、直接内存访问控制器配合模数转换接口以及内部集成电路总线,并行采集电流、电压及四元数数据,并通过滑动窗口滤波消除高频噪声;在不额外占用中央处理器算力的前提下,构建了时间同步、多维度的底层数据缓冲矩阵,为后续的高精闭环控制提供了可靠的数据基础。

23、优选地,所述s2中计算实时扭矩梯度的具体步骤包括:

24、s211、设定离散时间步长;从滑动窗口缓冲数组中提取当前时间步的实时扭矩反馈值与上一时间步的实时扭矩反馈值;

25、s212、将当前时间步的实时扭矩反馈值减去上一时间步的实时扭矩反馈值,得到扭矩变化量;

26、s213、将扭矩变化量除以离散时间步长,得到实时扭矩梯度;

27、本发明通过提取离散时间步内的扭矩变化量并除以时间步长,将静态的扭矩值转化为动态的扭矩变化速率;能够精准捕捉绞刀接触硬物或被渔网瞬间缠绕时的机械阻力突变特征,有效滤除了常规切削过程中的稳态扭矩波动,提高了异常工况识别的准确率。

28、优选地,所述s2中控制绞刀电机执行动态脱困序列的具体步骤包括:

29、s221、向履带电机下发速度为零的停止指令,向泥浆泵下发设定的低频保护频率指令;

30、s222、生成占空比为百分之百的反转脉宽调制信号,控制绞刀电机以最大反向扭矩反转设定的第一持续时间;

31、s223、在第一持续时间结束后,生成非对称正反转抖动指令;非对称正反转抖动指令包括正转设定的第二持续时间与反转设定的第三持续时间,且第三持续时间大于第二持续时间;

32、s224、控制绞刀电机交替执行正转与反转,在每次交替执行期间,持续监测实时扭矩反馈值;

33、s225、判断实时扭矩反馈值是否下降至安全扭矩阈值以下;

34、若是,判定脱困成功,恢复执行步骤s3;

35、否则,判断交替执行的次数是否达到最大抖动次数阈值;若是,触发声光报警并向岸基上位机发送停机求救信号;否则,返回执行步骤s224;

36、本发明在识别到缠绕卡死时,首先切断履带推进动力防止缠绕加剧,随后利用占空比百分之百的反向脉宽调制信号释放瞬时最大反向扭矩以撕裂缠绕物;若未能脱困,则引入第三持续时间大于第二持续时间的非对称正反转抖动指令,利用非对称机械惯性逐步将缠绕物从绞刀轴上旋退剥离;上述多级动态脱困序列无需人工干预,在保护电机及驱动电路免受持续大电流冲击的同时,最大化了水下自主挣脱的成功率。

37、优选地,所述s3中浓度偏差的计算具体步骤包括:

38、s311、调取泥浆泵的最佳泵送效率曲线,提取最佳泵送效率曲线的极值点对应的浓度数值作为预设目标泥浆浓度;

39、s312、将当前时间步的实时泥浆浓度值减去预设目标泥浆浓度,得到当前时间步的浓度偏差;

40、s313、将当前时间步的浓度偏差减去上一时间步的浓度偏差,得到浓度偏差变化率;

41、本发明以泥浆泵最佳泵送效率曲线的极值点作为预设目标泥浆浓度,通过计算当前时间步的浓度偏差及浓度偏差变化率,将抽象的清淤效能具象化为可量化的控制输入误差;确立了系统寻优的基准靶点,确保泥浆泵始终运行在高效、防堵塞的流体力学区间内。

42、优选地,所述s3中计算基础绞刀转速调节量与基础履带推进速度调节量的具体步骤包括:

43、s321、将当前时间步的浓度偏差与浓度偏差变化率输入模糊比例积分微分控制器;

44、s322、模糊比例积分微分控制器利用预设的模糊规则矩阵对当前时间步的浓度偏差与浓度偏差变化率进行模糊推理,输出比例系数增量、积分系数增量与微分系数增量;

45、s323、将基础比例系数加上比例系数增量得到当前比例系数,将基础积分系数加上积分系数增量得到当前积分系数,将基础微分系数加上微分系数增量得到当前微分系数;

46、s324、利用当前比例系数、当前积分系数与当前微分系数,结合当前时间步的浓度偏差、上一时间步的浓度偏差与上两时间步的浓度偏差,通过增量式比例积分微分公式计算得出总控制增量;

47、s325、将总控制增量乘以第一分配权重得到基础绞刀转速调节量,将总控制增量乘以第二分配权重得到基础履带推进速度调节量;

48、本发明将浓度偏差与变化率输入模糊比例积分微分控制器,通过模糊推理矩阵在线动态修正比例、积分与微分系数,进而利用增量式公式计算总控制增量,并按权重分配至绞刀与履带;克服了传统固定参数控制在面对底泥密度突变时响应迟缓或超调震荡的缺陷,实现了绞刀切削量与履带进给量的自适应动态匹配,确保了泥浆浓度的快速收敛与稳定。

49、优选地,所述s322中预设的模糊规则矩阵的设定规则为:

50、当浓度偏差为负大且浓度偏差变化率为负大时,输出正大的比例系数增量与零的积分系数增量;

51、当浓度偏差为正大且浓度偏差变化率为正大时,输出负大的比例系数增量与负大的积分系数增量;

52、本发明的模糊规则在浓度严重偏低且持续恶化时,输出正大比例增量并关闭积分作用;在浓度严重偏高时反向削弱控制量。此非线性映射规则有效抑制了积分饱和现象,提升了系统在极端底泥密度分布下的动态响应速度与鲁棒性。

53、优选地,所述s4中液压推杆补偿行程的计算具体步骤包括:

54、s411、获取预先设定的水平基准角,将实时俯仰角减去水平基准角,得到姿态偏差;

55、s412、提取绞刀大臂的物理长度参数;

56、s413、将姿态偏差转换为弧度制,计算姿态偏差的正弦值;将姿态偏差的正弦值乘以物理长度参数,得到绞刀大臂末端的垂直高度位移量;

57、s414、将垂直高度位移量乘以液压缸传动比系数,计算得出液压推杆补偿行程;

58、本发明将实时俯仰角与水平基准角的差值转换为弧度,结合绞刀大臂的物理长度参数计算垂直高度位移量,并乘以液压缸传动比系数得出补偿行程;精确量化了车体俯仰引起的大臂末端悬空或扎泥误差,为后续的液压执行机构提供了精确的物理位移控制目标。

59、优选地,所述s4中调节绞刀大臂的升降高度的具体步骤包括:

60、s421、提取液压比例阀的控制增益参数,将液压推杆补偿行程除以控制增益参数,计算得出目标脉冲宽度;

61、s422、利用stm32微控制器的高级定时器根据目标脉冲宽度生成脉宽调制信号;

62、s423、将脉宽调制信号输出至液压比例阀的控制端,驱动液压推杆执行液压推杆补偿行程;

63、本发明将液压推杆补偿行程除以控制增益参数转换为目标脉冲宽度,利用高级定时器生成脉宽调制信号驱动液压比例阀;基于前馈补偿的硬件驱动方式,消除了地形起伏对清淤深度的干扰,实现了水下绞刀的恒定贴底仿形作业。

64、另一方面,本发明还提供基于stm32的清淤控制系统,用于实现所述基于stm32的清淤系统控制方法,所述系统包括:

65、多源数据采集模块,用于通过stm32微控制器上的外设接口同步采集多源作业数据;

66、异常工况脱困模块,用于计算实时扭矩梯度,在判定进入缠绕卡死异常工况时控制绞刀电机执行动态脱困序列;

67、自适应协同控制模块,用于在正常清淤工况下,利用模糊比例积分微分控制器计算基础调节量,并结合实时推力反馈值与实时扭矩反馈值分配指令;

68、贴底补偿控制模块,用于计算液压推杆补偿行程,并输出脉宽调制信号调节绞刀大臂的升降高度。

69、(三)有益效果

70、本发明具有以下有益效果:

71、本发明通过stm32微控制器同步采集多模态物理量,引入了扭矩一阶时间导数计算;将静态的电流幅值转化为动态的机械阻力梯度,能够识别绞刀瞬间切入硬物或被渔网缠绕的异常工况;配合系统自动生成的全功率反转与非对称正反转抖动脱困序列,本发明在保护电机免受持续堵转大电流冲击的同时,利用机械惯性撕裂并旋退缠绕物,解决了传统清淤设备遇险脱困难、易烧毁电机的行业痛点。

72、本发明在正常清淤工况下,以泥浆泵最佳泵送效率对应的浓度极值点为靶向,引入模糊比例积分微分控制器;系统根据浓度偏差及变化率动态修正控制系数,并将计算得出的总控制增量按物理功率权重科学分配至绞刀与履带;结合基于极限附着力的安全推力衰减与基于热限的扭矩裕量约束,本发明实现了切削量与进给量的自适应动态匹配,有效抑制了履带打滑与电机过载,确保泥浆浓度始终稳定在最高效的流体力学区间。

73、本发明针对水下复杂地形,构建了独立于浓度控制的姿态前馈补偿机制;通过六轴传感器获取实时俯仰角,结合大臂杠杆几何传动比,利用三角函数精确解算出液压推杆的直线补偿行程,并通过微控制器定时器直接驱动液压比例阀;消除了地形起伏对清淤深度的物理干扰,实现了绞刀的恒定贴底仿形作业,大幅减少了悬空吸水现象,保障了清淤作业的高效性与连续性。

74、当然,实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。