一种引入重量变量控制的无人农机协同工作控制方法与流程

1.本发明属于惯性导航、陀螺仪误差补偿领域，具体涉及一种引入重量变量控制的无人农机协同工作控制方法。


背景技术：

2.近年来，随着农机自动化技术、智慧农业体系不断地发展，我国农业初步形成了耕、种、管、收全程数字化管理模式。无人农场就是要实现耕种管收生产环节全覆盖、机库田间转移作业全自动、自动避障、异况停车、作物生产过程实施全监控、智能决策精准作业全无人。我国目前已经先后研制成功无人驾驶旋耕机、无人驾驶播种机、无人驾驶插秧机、我人家是高地隙宽幅喷雾机和无人驾驶收获机等多种智能农机。
3.随着中国农业集约化、规模化、产业化的发展以及多作业场景需求的不断提出，无人农机的物料输送智能化是制约无人农场效率的关键因素。物料的输送包括四个阶段：会合阶段，当受料机的物料无法满足任务需求时，需要像加料机发送信号并在按照约定的集结地点和路径会合；对接阶段，对接是物料自动输送的关键，因加料机与收料机在输送过程中均为动态且农田内环境较为复杂，所以需要保证加料机与收料机物料入口的精准匹配和对接；输送阶段，对接完成后物料开始输送，由于输料过程中，加料机质量不断下降，收料机重量上升，因此必须实时调整双方的控制参数，保证输料过程稳定；分离阶段，加料结束信号发出后，输送设备停止工作，将输料接口分离收回，收料机继续工作，加料机返回或继续下一个加料任务。前农场中收料机与加料机规划为单机固定路径规划，收料机与加料机需要到固定点进行加料操作，因此需要停止收料机工作状态，降低了工作效率；目前多机协同作业路径规划及跟随的难度较大，单一的路径规划又难以得到较好的工作效果。


技术实现要素：

4.为解决上述技术问题，本发明提供了一种引入重量变量控制的无人农机协同工作控制方法，本方法将控制分为上下两层，上层控制器主要为接收双车的位置、速度及加速度航向等信息，通过上述信息求解出期望的车辆加速度；下层传感器接收期望加速度，并转化为控制信号控制车辆油门及制动。
5.本发明建立加料机与收料机之间的运动学模型，建立双车间上层控制模型以此输出车辆的期望控制值，然后通过车辆纵向动力学分析建立车辆下层控制模型，引入车辆重量作为控制参量，优化车辆自身控制，保证稳定跟车状态。
6.为达到上述目的，本发明采用的技术方案为：一种引入重量变量控制的无人农机协同工作控制方法，包括如下步骤：步骤（1）确定加料车与收料车之间的稳定关系，建立以距离、速度及加速度为控制核心的模型；步骤（2）考虑加速度的控制执行延时，建立加速度延时表达式；步骤（3）建立给料车控制状态空间的离散化模型，进行预测时间范围内的给料车
状态的状态预测；步骤（4）通过建立性能指标函数和偏差，得到二次规划标准型，通过有效集的方法求解出二次规划值，获得期望加速度；步骤（5）车辆下层控制系统得到期望加速度后，选择驱动或制动模式，将车辆重量作为控制参数，获得节气门开度或主缸压力，进而控制车辆速度。
7.进一步地，所述步骤（1）中，输料过程中，收料车按照既定路线前进，加料车以收料车的位姿作为参考实时调整运动姿态来保证两车之间位姿相对不变；以收料车的几何中心点位参考点，收料车与加料车有固定的距离；因此，为保证传料稳定性，两车距离误差及速度误差保持接近0，表达式如下：，，，式中，为辆车位置间距误差，为主车位置，为从车位置，为两车期望距离，为初始两车间距，为时间常数，可根据国家相关标准取得，为主车与从车速度，为两车速度误差；将车辆简化为只有速度和位置的质点，准确描述车辆的运动状态，给料车的运动状态表示为：，，式中，为加速度。
8.进一步地，所述步骤（2）中的加速度延时表达式表示为：，式中，为实际加速度，为期望加速度，为时间常数，为复频域标志。
9.进一步地，所述步骤（3）的建立给料车控制状态空间的离散化模型包括：，，
式中，，，，，为系统输入量加速度值，为单位矩阵，y为测量到的给料车状态，t为采样周期；为k时刻系统输入量，为系统噪声；将加速度变化率的约束引入系统，建立新的状态量，则系统模型对应表示为：，，式中，，，，，为系统输入量加速度值增量，为测量到的给料车状态；通过上式进行预测时间范围内的给料车状态的状态预测，则得到预期的控制输入为：，其中，为上一时刻的输入，为已知量，为增量输入，通过二次规划求得，j为当前时刻与初始时刻间隔。
10.进一步地，所述步骤（4）包括：定义性能指标函数为：，为k时刻预测值，为k时刻参考值，为控制输入增量；q为误差加权矩阵，r为控制加权矩阵，n为模型时域；，其中，分别为矩阵对应变量矩阵形式，预测步数。
11.定义预测值与参考量的偏差：
，通过对性能指标函数j变形，可得到二次规划标准型：，其中，为实向量；通过有效集的方法求解出二次规划值，在每个控制周期内得出下一时刻系统最优的控制序列；为防止预测误差随时间增大造成控制系统不稳定，因此选择第一个量作为当前期望加速度增量，表达式如下：，其中，为控制输入增量。
12.进一步地，所述步骤（5）包括：获得期望加速度值后，上层控制器将数据发送给下层控制部分，下层控制器部分根据车辆驱动和制动下不同模型转换为可执行的指令；车辆驱动模式下控制模型为：，其中，为t时刻期望发动机转矩，为空气阻力系数，为滚动阻力系数，为重力加速度，为纵向车速，为车辆迎风面积，为空气密度，为车轮半径，为变速器传动比，为主减速器传动比，为传动系机械效率，根据给料车当前重量m及期望加速度等参数即可求解出期望的节气门开度；制动模式下车辆动力学模型为：，其中，为期望制动力，根据给料车当前重量及车辆相关制动参数可求解出期望的制动主缸压力如下：，其中，为期望的制动主缸压力，为前轮制动器的制动力矩与制动主缸压力的比例系数，为后轮制动器的制动力矩与制动主缸压力的比例系数。
13.与传统差分定位技术相比，本发明的优点在于：建立加料机与收料机之间的运动学模型，建立双车间上层控制模型以此输出车辆的期望控制值，然后通过车辆纵向动力学分析建立车辆下层控制模型，引入车辆重量作为控制参量，优化车辆自身控制，保证稳定跟车状态。
附图说明
14.图1为本发明的一种引入重量变量控制的无人农机协同工作控制方法流程图。
具体实施方式
15.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
16.本发明建立加料机与收料机之间的运动学模型，建立双车间上层控制模型以此输出车辆的期望控制值，然后通过车辆纵向动力学分析建立车辆下层控制模型，通过引入车辆重量作为控制参量，优化车辆自身控制，保证稳定跟车状态。
17.本发明的一种引入重量变量控制的无人农机协同工作控制方法具体包括如下步骤：步骤（1）确定加料车与收料车之间的稳定关系，建立以距离、速度及加速度为控制核心的模型。
18.步骤（2）考虑加速度的控制执行延时，建立加速度延时表达式。
19.步骤（3）建立给料车控制状态空间的离散化模型，进行预测时间范围内的给料车状态的状态预测。
20.步骤（4）通过建立性能指标函数和偏差，得到二次规划标准型，通过有效集的方法求解出二次规划值，获得期望加速度。
21.步骤（5）车辆下层控制系统得到期望加速度后，选择驱动或制动模式，将车辆重量作为控制参数，获得节气门开度或主缸压力，进而控制车辆速度。
22.具体地，输料过程中，收料车按照既定路线前进，加料车以收料车的位姿作为参考实时调整运动姿态来保证两车之间位姿相对不变。以收料车的几何中心点位参考点，收料车与加料车有固定的距离。因此，为保证传料稳定性，两车距离误差及速度误差保持接近0，表达式如下：，，，式中，为辆车位置间距误差，为主车位置，为从车位置，为两车期望距离，为初始两车间距，为时间常数，可根据国家相关标准取得，为主车与从车速度，为两车速度误差。
23.将车辆简化为只有速度和位置的质点，准确描述车辆的运动状态，给料车的运动状态可以表示为：，
，式中，为加速度；因计算部分给出控制指令到执行机构转换成可执行的动作之间有延时，所以实际加速度近似表示为：，式中，为实际加速度，为期望加速度，为时间常数，为复频域标志。
24.根据上述公式，建立给料车控制状态空间的离散化模型：，，式中，，，，，为系统输入量加速度值，为单位矩阵，y为测量到的给料车状态，t为采样周期；为k时刻系统输入量，为系统噪声。
25.将加速度变化率的约束引入系统，建立新的状态量，则系统模型对应表示为：，，式中，，，，，为系统输入量加速度值增量，为测量到的给料车状态。
26.通过上式可进行预测时间范围内的给料车状态的状态预测，则可以得到预期的控制输入为：，其中，为上一时刻的输入，为已知量，为增量输入，通过二次规划求得，j为当前时刻与初始时刻间隔。
27.定义性能指标函数为：，
其中，为k时刻预测值，为k时刻参考值，为控制输入增量；q为误差加权矩阵，r为控制加权矩阵，n为模型时域。
28.，分别为矩阵对应变量矩阵形式，预测步数。
29.定义预测值与参考量的偏差：，通过对性能指标函数j变形，可得到二次规划标准型：，其中，为实向量；通过有效集的方法求解出二次规划值，在每个控制周期内可以得出下一时刻系统最优的控制序列。为防止预测误差随时间增大造成控制系统不稳定，因此选择第一个量作为当前期望加速度增量，表达式如下：，其中，为控制输入增量；获得期望加速度值后，上层控制器将数据发送给下层控制部分，下层控制器部分根据车辆驱动和制动下不同模型转换为可执行的指令。
30.车辆驱动模式下控制模型为：，其中，为t时刻期望发动机转矩，为空气阻力系数，为滚动阻力系数，为重力加速度，为纵向车速，为车辆迎风面积，为空气密度，为车轮半径，为变速器传动比，为主减速器传动比，为传动系机械效率，根据给料车当前重量m及期望加速度等参数即可求解出期望的节气门开度；制动模式下车辆动力学模型为：，其中，为期望制动力，根据给料车当前重量m及车辆相关制动参数可求解出期望的制动主缸压力如下：，
其中，为期望的制动主缸压力，为前轮制动器的制动力矩与制动主缸压力的比例系数，为后轮制动器的制动力矩与制动主缸压力的比例系数。
31.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，包括如下步骤：步骤（1）确定加料车与收料车之间的稳定关系，建立以距离、速度及加速度为控制核心的模型；步骤（2）考虑加速度的控制执行延时，建立加速度延时表达式；步骤（3）建立给料车控制状态空间的离散化模型，进行预测时间范围内的给料车状态的状态预测；步骤（4）通过建立性能指标函数和偏差，得到二次规划标准型，通过有效集的方法求解出二次规划值，获得期望加速度；步骤（5）车辆下层控制系统得到期望加速度后，选择驱动或制动模式，将车辆重量作为控制参数，获得节气门开度或主缸压力，进而控制车辆速度。2.根据权利要求1所述的一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，所述步骤（1）中，输料过程中，收料车按照既定路线前进，加料车以收料车的位姿作为参考实时调整运动姿态来保证两车之间位姿相对不变；以收料车的几何中心点位参考点，收料车与加料车有固定的距离；因此，为保证传料稳定性，两车距离误差及速度误差保持接近0，表达式如下：，，，式中，为辆车位置间距误差，为主车位置，为从车位置，为两车期望距离，为初始两车间距，为时间常数，为主车与从车速度，为两车速度误差；将车辆简化为只有速度和位置的质点，准确描述车辆的运动状态，给料车的运动状态表示为：，，式中，为加速度。3.根据权利要求2所述的一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，所述步骤（2）中的加速度延时表达式表示为：，式中，为实际加速度，为期望加速度，为时间常数，为复频域标志。4.根据权利要求3所述的一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，所述步骤（3）的建立给料车控制状态空间的离散化模型包括：，
，式中，，，，，为系统输入量加速度值，为单位矩阵，y为测量到的给料车状态，t为采样周期；为k时刻系统输入量，为系统噪声；将加速度变化率的约束引入系统，建立新的状态量，则系统模型对应表示为：，，式中，，，，，为系统输入量加速度值增量，为测量到的给料车状态；通过上式进行预测时间范围内的给料车状态的状态预测，则得到预期的控制输入为：，其中，为上一时刻的输入，为已知量，为增量输入，通过二次规划求得，j为当前时刻与初始时刻间隔。5.根据权利要求4所述的一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，所述步骤（4）包括：定义性能指标函数为：，为k时刻预测值，为k时刻参考值，为控制输入增量；q为误差加权矩阵，r为控制加权矩阵，n为模型时域；，其中，分别为矩阵对应变量矩阵形式，预测步数；定义预测值与参考量的偏差：
，通过对性能指标函数j变形，可得到二次规划标准型：，其中，为实向量；通过有效集的方法求解出二次规划值，在每个控制周期内得出下一时刻系统最优的控制序列；为防止预测误差随时间增大造成控制系统不稳定，因此选择第一个量作为当前期望加速度增量，表达式如下：，其中，为控制输入增量。6.根据权利要求5所述的一种引入重量变量控制的无人农机协同工作控制方法，其特征在于，所述步骤（5）包括：获得期望加速度值后，上层控制器将数据发送给下层控制部分，下层控制器部分根据车辆驱动和制动下不同模型转换为可执行的指令；车辆驱动模式下控制模型为：，其中，为t时刻期望发动机转矩，为空气阻力系数，为滚动阻力系数，为重力加速度，为纵向车速，为车辆迎风面积，为空气密度，为车轮半径，为变速器传动比，为主减速器传动比，为传动系机械效率，根据给料车当前重量m及期望加速度等参数即可求解出期望的节气门开度；制动模式下车辆动力学模型为：，其中，为期望制动力，根据给料车当前重量及车辆相关制动参数可求解出期望的制动主缸压力如下：，其中，为期望的制动主缸压力，为前轮制动器的制动力矩与制动主缸压力的比例系数，为后轮制动器的制动力矩与制动主缸压力的比例系数。

技术总结
本发明公开了一种引入重量变量控制的无人农机协同工作控制方法，该方法将控制分为上下两层，建立加料机与收料机之间的运动学模型，建立双车间上层控制模型以此输出车辆的期望控制值，然后通过车辆纵向动力学分析建立车辆下层控制模型，通过引入车辆重量作为控制参量，优化车辆自身控制，保证稳定跟车状态。保证稳定跟车状态。保证稳定跟车状态。


技术研发人员：刘振耀 楚焕鑫 李子申 汪亮 王宁波 刘炳成 蔚科 尹心彤
受保护的技术使用者：齐鲁空天信息研究院
技术研发日：2023.07.06
技术公布日：2023/8/5