大型喷杆滚转运动控制用的电液主动悬架系统与控制方法与流程

本发明属于农业机械技术领域，具体涉及一种大型喷杆滚转运动控制用的电液主动悬架系统与控制方法。

背景技术：

喷杆式喷雾机雾化效果好，喷幅宽，作业效率高，是一种高效的大田植保机械。随着喷杆喷雾机逐渐大型化，喷雾机行驶时，地面不平整及土壤形变、车速变化、药液晃动等产生的随机激励，会更容易通过底盘传递到喷杆，喷杆的主要运动表现为滚转、摇摆、振荡等不规律运动。其中，显著影响喷雾分布均匀性的是喷杆的滚转运动。

喷杆滚转运动会造成喷嘴与作物高度发生变化，致使施药质量下降、喷杆易于损坏的问题日益突出。常规的喷杆被动悬架系统主要用于衰减来底盘的运动干扰，不能实现地形跟踪，而且当扰动激励与悬架固有频率接近时，悬架易产生共振。喷杆主动悬架系统可以持续提供和调整改变喷杆运动状态所需的驱动力，使喷杆始终和起伏变化的地面平行。主动悬架主要由动力源、执行器、非接触测距传感器、电控单元等组成。接近式测距传感器实时测量喷杆到作物冠层或者地面的距离，距离信号通过滤除噪声及处理，控制器给执行器提供适当的驱动信号，液压缸以一定的速度伸长或者缩短，使得喷杆的重心落在单摆转轴的左边或者右边，此时重力、弹簧力产生回复力矩，使喷杆的重心回到转轴的下方，单摆和铅垂线重合。主动悬架系统的优势在于可使喷杆上喷嘴到靶标(地面或者作物冠层)的距离保持稳定，不受作物冠层高度、地形、底盘晃动的影响，保持最佳的喷杆高度有利于实现均匀喷洒、抑制雾滴飘移。同时，主动悬架还可以减轻驾驶员的负担，不必频繁的根据地形调整喷杆角度，提高作业效率。但目前国内外对主动悬架的使用还存在以下问题，例如喷杆臂展长，属于弱阻尼、大惯量结构；喷杆高度测量通常选用的超声波测距传感器，在复杂农田环境中存在测量频响低，声波容易发生多次反射、噪音干扰、交叉干扰的情况；喷幅范围内地表构造粗糙不平，覆盖了较大的空间频率范围；作物生长期冠层高度分布不均等，这些问题增加了对喷杆主动控制的难度，

技术实现要素：

针对现有技术存在的问题，本发明提供了一种新型的可大型喷杆滚转运动控制用的电液主动悬架系统，便于控制，且可以有效的控制喷杆滚转运动，同时针对所述电液主动悬架系统提供了控制精度高的控制算法，可显著提高喷杆工作的稳定性，提升喷洒作业质量。

本发明的技术方案为：

一种大型喷杆滚转运动控制用的电液主动悬架系统，其特征在于，包括控制系统和电液悬架系统；

所述电液悬架系统包括悬架装置和传感器组，所述悬架装置包括支撑架、主动液压缸、第一减振器、辅助连杆、喷杆、摆杆和第二减振器；

所述支撑架与喷雾机尾部车架固定连接，支撑架由一横杆和左右两根竖杆构成，两竖杆垂直固定在横杆下方；

所述摆杆用于连接喷杆和支撑架，摆杆的顶端与支撑架横杆的中部通过关节副铰接，摆杆的底端通过关节副与喷杆的中部铰接，喷杆横向摆放；

所述辅助连杆设置在喷杆的一侧，呈形，由一段竖杆和一段横杆组成，辅助连杆的竖杆固定在喷杆上，处于喷杆外端与摆杆之间，辅助连杆竖杆的顶部高度低于摆杆的顶端；辅助连杆的横杆固定在其竖杆中部的外侧；

所述主动液压缸用于连接辅助连杆和摆杆，主动液压缸的一端通过关节副与摆杆上部的b点铰接，所述b点处于摆杆顶端的下方，主动液压缸的另一端通过关节副与辅助连杆竖杆的顶端铰接；所述主动液压缸与控制系统连接，控制系统通过对主动液压缸伸缩运动的控制实现对喷杆滚转运动的控制，即喷杆相对水平面侧倾角的自动调整；

所述第一减振器、第二减振器均为可变阻尼减振器，所述第一减振器与辅助连杆位于同一侧，其一端通过关节副与支撑架对应侧的竖杆铰接，另一端通过关节副与辅助连杆的横杆铰接；第二减振器的一端通过关节副与支撑杆对应侧的竖杆铰接，另一端通过关节副与摆杆上的g点铰接，所述g点位于上述b点的下方；所述第一减振器用于缓冲主动液压缸的调节，防止主动液压缸作动过大或者过快激发喷杆的振动；第二减振器用于稳定摆杆，为主动液压缸动作提供反作用力；

上述结构中，所述关节副由转轴和连接转轴的轴承座或轴座组成，所述转轴平行于喷雾机的前进方向；

所述传感器组包括第一测距传感器、第二测距传感器和第一动态倾角传感器、第二动态倾角传感器；

所述第一动态倾角传感器安装在支撑架上，用于检测支撑架水平侧倾角；第二动态倾角传感器安装在喷杆上，用于检测喷杆水平侧倾角；所述第一测距传感器安装在喷杆的左喷臂上，用于检测喷杆左侧测点距离地面的高度；第二测距传感器检测安装在喷杆的右喷臂上，用于检测喷杆右侧测点距离地面的高度。

在上述方案的基础上，进一步改进或优选的方案还包括：

所述第一减振器、第二减振器为通过弹簧复位的液压阻尼器，所述液压阻尼器活塞两端的液压腔室通过设有比例流量阀的管路连通，所述比例流量阀的控制信号输入端与所述控制系统连接，由控制系统调节开度，进而实现对第一减振器、第二减振器阻尼大小的调整。

通过设置在主动液压缸驱动液路上的液压控制阀控制主动液压缸的动作，所述液压控制阀的控制信号输入端与控制系统连接。

控制系统对主动液压缸的伸缩与第一减振器、第二减振器的阻尼大小同时耦合控制。

所述第一、第二动态倾角传感器优选采用5hz以上的高频响动态倾角传感器。

一种基于所述电液主动悬架系统的控制大型喷杆滚转运动的控制方法，控制系统通过液压控制阀控制主动液压缸的运动，通过比例流量阀实现对减振器阻尼大小的调节，所述控制方法，其特征在于，包括：

所述控制系统包括上位机和下位机控制器，下位机控制器接收传感器的反馈信号以及上位机的发送的控制参数，根据预设的控制算法获取用于所述液压控制阀的控制指令输出值u、用于第一比例流量阀的控制指令输出值ul1、用于第二比例流量阀的控制指令输出值ul2；

其中：

式中，k11、k12、k13均为比例系数；β1是第一动态倾角传感器的反馈数据(即支撑架的水平侧倾角)，β2是第二动态倾角传感器的反馈数据，β2是第二动态倾角传感器检测的喷杆的水平侧倾角，表示β2的二阶导数的绝对值，即喷杆侧倾角加速度的绝对值；v表示喷雾机作业时的行驶速度；|u|是控制主动液压缸运动的液压控制阀的控制指令输出值u的绝对值；b偏置系数，b取值的大小决定了第一减振器在喷雾机静止时的阻尼系数的大小；*表示乘号；

v增大时，喷雾机的振动变大，进一步导致支撑架的振动变大，调整参数时，通过增大k12降低第一减振器的阻尼，以降低有害振动的传递；增大时，通过调大k11增大第一减振器的阻尼，抑制喷杆滚转运动；k13的取值根据喷杆相对支撑架的转角(β2-β1)的大小进行调整，(β2-β1)增大时，调大k13，增大第一减振器的阻尼；

k14是根据|u|的大小决定的系数，当|u|增大时，通过增大k14减小第一减振器的阻尼，降低第一减振器对主动液压缸运动的阻碍作用；

式中，k21和k22表示比例系数，表示β1的二阶导数的绝对值，即支撑架侧倾角加速度的绝对值；

行驶速度v增大，调整参数时，通过增大k22降低第二减振器的阻尼，以减小有害振动的传递；增大时，通过减小k21增大第二减振器的阻尼，抑制喷杆的滚转运动；

k23是根据|u|的大小决定的系数，当|u|增大时，通过增大k23，增大第二减振器的阻尼，增加摆杆的支撑刚度。

进一步的，所述控制指令值u可通过如下公式计算：

u＝uf+ud+ua

上式中，

ud＝kd*(p1-p2)；

β3为根据第一、第二测距传感器的反馈信号换算出的喷杆相对地面的侧倾角度，h1为第一测距传感器的反馈数据(即第一测距传感器所在喷杆测点距离地面的高度)，h2为第二测距传感器的反馈数据(即第二测距传感器所在喷杆测点距离地面的高度)，d为两个测距传感器在喷杆上的安装距离，表示β3的一阶导数，即喷杆相对地面的侧倾角速度；

将β3与(β2-β1)进行数据融合后获得的反馈信号值βd，k表示采样序列，k＝1，2，...，βd(k)表示第k次采样计算得的角度反馈信号值；

p1为所述主动液压缸的进口压力，p2为所述主动液压缸的出口压力；

kf是前馈补偿系数，kd是动压反馈系数，ka1和ki分别是比例系数和积分系数。

设液压控制阀控制指令输出值u为-10v～+10v,对应其阀芯开度-100％～100％，当u的计算值大于10v时，另u＝10v；当u的计算值小于-10v时，另u＝-10v；

设减振器使用的比例流量阀控制指令输出值ul1、ul2为0～10v,对应阀芯开度0％～100％，当ul1、ul2的计算值大于10v时，则令ul1＝10v，ul2＝10v。

对传感器信号的处理包括：

定义δβ＝(β2-β1)；

如果β3＞pg，且则使用数据融合算法，将β3与喷杆相对支撑架的角度δβ进行数据融合，使用数据融合后获得的βd作为有效输入角度反馈给控制器；为δβ的一阶导数，即喷杆相对支撑架转动的角速度；

如果角速度的绝对值则使用β3作为有效输入角度反馈给控制器；

pg和pf表示是否启用数据融合的分割系数，将通过试验多次调试来获取最佳值。

所述试验的步骤包括：

(1)选择一块种植作物的条田，作物的高度在20-80厘米之间；

(2)将喷雾机开到田头，车轮对准行间距，展开喷杆，调至水平状态；

(3)设置两种特殊工况，第一种工况是在车轮行进前方，一侧车轮行驶的地面上布置不低于20厘米的凸台；第二中工况是在车轮行进前方，找一处有作物倒伏的地块，使喷杆一侧的超声波传感器能从倒伏作物上方经过；

(4)将动态倾角传感器，安装在喷杆的中心位置，测量喷杆的滚转角加速度和垂直加速度；

(5)设定pg等于0.1,pf的设定值从0.1开始试验，依次经过两种试验工况测试，分别记录喷杆的角加速度值和垂直加速度；

(6)依次增大pf，开展多组试验，直到喷杆的角加速度值和垂直加速度的加权平均值最小，记录pg和pf的数值；

(7)依次设定pg等于0.2,0.3,...5，重复步骤(5)和步骤(6)。

(8)选择试验组中喷杆角加速度值和垂直加速度的加权平均值最小的一组,即pg和pf的最佳值。

有益效果：

本发明电液主动悬架系统通过合理的规划和设计，在对悬架结构进行改进的基础上，以闭环控制的方式，通过传感器采集喷杆的运动状态，通过改进设计的前馈补偿运动控制算法解算出驱动指令，对悬架结构不同部位的阻尼和运动进行同步耦合控制，可有效的抑制喷杆的滚转运动，保障喷杆工作的稳定性，提升喷雾机的喷洒作业质量。

附图说明

图1为电液主动悬架系统的结构示意图；

图2为电液悬架系统的结构示意图；

图3为液压减振器的剖面图；

图4为自动控制主程序的流程图。

具体实施方式

为了进一步阐明本发明的技术方案和工作原理，下面结合附图与具体实施例对本发明做详细的介绍。

根据高等动力学中刚体运动的描述方法，文中喷杆滚转运动是指喷杆绕坐标系纵轴的旋转运动，通常也称“侧倾”，喷杆滚转角也称“侧倾角”，纵轴是指通过喷杆质心在喷杆对称平面内垂直于竖轴的坐标轴，沿喷雾机前进方向为正方向。本文中，所述的“左”“右”为参考方位，可知根据参照事物的变化，上述方位会产生相应的变化。

如图1所示，一种大型喷杆滚转运动控制用的电液主动悬架系统，包括控制系统和电液悬架系统。所述控制系统包含上位机和下位机，所述电液悬架系统由悬架装置及传感器等组成。

所述上位机由人机界面、通信接口及触摸屏硬件等构成，所述下位机采用基于dsp的控制器。用户通过人机界面向上位机输入指令，实现对喷杆手动工作模式、喷杆自动工作模式的选择，以及启动施药状态监测、控制参数设计、工作数据记录等功能。上位机通过modbus协议将用户的指令发送给下位机控制器。下位机将监测系统状态的传感器数据采集回来发给上位机，可以在线显示系统运动状态。

所述下位机的硬件电路包括处理器、a/d采集电路、d/a输出电路、通讯电路等。为了保证液压伺服控制系统的实时性及稳定性，优先选用高速dsp芯片作为系统控制核心处理器，所述a/d采集电路用于采集模拟量传感器(测距传感器、主动液压缸的位置、压力传感器)的输出信号，并将该信号转换为dsp可以运算的数字信号。dsp芯片通过设计的滤波算法处理，得到喷杆运动状态的反馈信号，再经设计的前馈补偿运动控制算法解算出驱动指令信号，经d/a输出电路转化为电压信号，用以控制液压控制阀的流量，驱动主动液压缸以期望速度运动到达期望的位置，所述液压控制阀优先使用电液伺服阀或电液比例阀，本实施例中选择了伺服阀。同时，设计sci串行通信电路，用于与上位机人机界面通信和对惯性姿态传感器信号的采集。所述sci串行通信电路的sci串口采用rs232通信模式，控制器包含两路通信串口scia和scib。scia波特率设置为115200bit/s，通信协议为modbus协议，其主要完成下位机控制器与人机界面的通信功能，包括接收人机界面发送的指令信号，以及实时向人机界面发送系统状态值以便上位机存储试验数据。本实施例中，所述惯性姿态传感器采用动态倾角传感器，优选5hz以上的高频响动态倾角传感器。scib用于向动态倾角传感器发送初始化指令，并接收动态倾角传感器发来的喷杆角度和角速度等信号，采用rs232串行通信协议，波特率也为115200bit/s。

如图1、图2所示，所述电液悬架系统包括支撑架1、主动液压缸2、第一减振器3、辅助连杆4、第一测距传感器5、第二动态倾角传感器6、第二测距传感器7、喷杆8、摆杆9、第二减振器10、第一动态倾角传感器11和伺服阀、比例流量阀等组成。本实施例中，所述第一、第二测距传感器采用超声波传感器。

所述支撑架1与喷雾机尾部车架固连，承载整个电液悬架系统的重力。支撑架1由一横杆和左右两个竖杆构成，两竖杆垂直固定在横杆端部的下方，结构对称。

所述摆杆9用于连接喷杆8和支撑架1，摆杆9的顶端与支撑架1横杆的中部(a点)通过关节副铰接。所述关节副由轴承座或轴座与平行于喷雾机前进方向的转轴组成，下同。摆杆9的底端通过关节副与喷杆8的中部(c点)铰接。

所述喷杆8横向设置，其左、右喷臂的结构对称，其长度方向与喷雾机前进方向垂直。如图2所示，喷杆8左侧喷臂的上面设置有一个辅助连杆4，所述辅助连杆4呈“┤”形，由一段竖杆和一段横杆组成。辅助连杆4的竖杆垂直固定在喷杆8上，处于喷杆8外端与摆杆9之间，其顶部高度低于摆杆9的顶端。辅助连杆4的横杆固定在辅助连杆4竖杆外侧的中部(e点)，与喷杆8平行。

所述主动液压缸2用于连接辅助连杆4和摆杆9，主动液压缸2的一端通过关节副与摆杆9的b点铰接，b点位于上述a点的下方，主动液压缸2的另一端通过关节副与辅助连杆4竖杆的顶端(d点)铰接。主动液压缸的伸缩可以实现喷杆8与摆杆9夹角的调整，从而改变喷杆8与水平面的夹角。

所述第一、第二减振器均为可变阻尼减振器，其中，第一减振器3两端分别通过关节副与支撑架1的左侧竖杆和辅助连杆4的横杆铰接，第二减振器10的两端分别通过关节副与支撑杆1的右侧竖杆和摆杆9的g点铰接，所述g点位于上述b点的下方。所述第一减振器3用于缓冲主动液压缸2的调节，防止主动液压缸2动作过大或者过快激发喷杆8的振动，可以有效避免喷杆角度调整时出现超调；第二减振器10用于在主动液压缸2调节喷杆角度时，稳定摆杆9，且不会阻碍主动液压缸2的作动，为主动液压缸2动作提供反作用力。

所述第一动态倾角传感器11安装在支撑架1中间位置上，第二动态倾角传感器6安装在喷杆8的中间位置上。第一动态倾角传感器11检测支撑架1的水平侧倾角β1，第二动态倾角传感器6检测喷杆8的水平侧倾角β2。第一测距传感器5检测喷杆8左侧测点距离地面的高度h1，第二测距传感器7检测喷杆8右侧测点距离地面的高度h2。所述伺服阀用于对主动液压缸2运动速度、运动方向的控制，也可以采用比例阀等其它电液控制阀代替。所述第一比例流量阀和第二比例流量阀分别用于对第一减振器3和第二减振器10流量的调节。

上述电液主动悬架系统的控制方法采用闭环伺服控制，实施过程为：

将两个超声波测距传感器分别安装于喷杆左、右喷臂上，测量喷杆8左右两个测点距离地面的高度，经过数据滤波，再结合两个测距传感器的安装距离d，换算出喷杆8相对地面的倾斜角度再将β3与喷杆8相对支撑架1的倾斜角度(β2-β1))进行数据融合，融合以后的角度βd作为控制器的反馈信号。伺服阀的阀板上设有2个压力传感器，分别用于测量主动液压缸2的进口压力p1和出口压力p2，也作为反馈信号给控制器。

本发明设计了前馈补偿运动控制算法根据各个传感器的处理后的反馈信号，综合计算得出控制指令，通过d/a输出电路转换为驱动电流，发送给伺服阀和比例流量阀，实现对喷杆水平侧倾角的调节，使其实时跟踪地面坡度的变化。

所述前馈补偿运动控制算法中，控制指令输出值u包括3项，分别是前馈补偿项uf、动压反馈项ud、比例积分项ua，控制指令u(电压信号)的计算通过如下公式：

u＝uf+ud+ua；

上式中，

ud＝kd*(p1-p2)；

表示β3的一阶导数，即喷杆8相对地面侧倾的角速度；*表示乘号；k表示采样序列，k＝1，2，...，βd(k)表示第k次采样计算得的角度反馈信号；kf是前馈补偿系数；kd是动压反馈系数；ka1和ki分别是比例系数和积分系数。

这些系数参数的调整都遵循跟踪误差最小化的原则，向着误差减小的方向进行调整，这里的控制指令输出值u仅仅用于伺服阀的控制，进而驱动图2中主动液压缸2运动，实现对喷杆8整体角度的调整。

所述第一减振器3和第二减振器10设计结构如图3所示，二者结构相同，均为阻尼可调的液压阻尼器。以第二减振器10为例，其机构包括内置复位弹簧101、活塞103、活塞杆104和外置复位弹簧105，第二比例流量阀设置在连通活塞103两侧液压腔室的液路中，减振器的阻尼大小可以根据作业状况通过控制比例流量阀的开度进行自动调节。

在对两减振器的阻尼大小进行实时调节时，由下位机控制器发送电压指令给对应的比例流量阀，电压越大比例流量阀的开度越大，减振器的阻尼系数越小。

输出给第一比例流量阀(对应第一减振器3)的控制指令算法设计如下：

式中，k11、k12、k13均为比例系数，可通过人机界面设置；

b是常数，或者说偏置系数，b的大小决定了喷杆喷雾机静止时，减振器阻尼系数的大小，可通过人机界面设置；

v表示喷雾机作业时的行驶速度，通过安装在喷雾机上的gps天线和接收机可实时测量；喷雾机行驶速度v越大，喷雾机的振动越大，进一步导致支撑架的振动越大，悬架装置需要较小的阻尼，以减小有害振动的传递，调整参数时，可以增大k12；

表示β2的二阶导数的绝对值，即喷杆侧倾角加速度的绝对值；增大，则需要增大阻尼，抑制喷杆滚转运动，调整k11时可根据角加速度的大小决定，增大时，调大k11；k13根据喷杆相对支撑架的转角(β2-β1)的大小进行调整，(β2-β1)增大时，可调大k13；

|u|是伺服阀指令值u的绝对值；k14是根据|u|的大小决定的，当主动液压缸2需要调整时,控制器发指令u给伺服阀的驱动电路，当|u|增大时，我们需要减小第一减振器1的阻尼，降低第一减振器3对主动液压缸2运动的阻碍作用，因此当|u|增大时可以增大k14。

根据上述算法，最终实现第一减振器3的阻尼系数根据喷杆运动大小、行驶速度、相对转角等因素进行自适应。

对应第二减振器10的第二比例流量阀的控制算法设计如下：

式中，k21和k22表示比例系数，通过人机界面设置；

v表示喷雾机作业时的行驶速度，行驶速度v越大，支撑架1振动越大，悬架装置需要较小的阻尼，以减小振动传递，调整参数时可以增大k22；

表示β1的二阶导数的绝对值，即支撑架1侧倾角加速度的绝对值，越大，需要增大第二减振器10的阻尼，抑制支撑架1的滚转运动，所以k21调整时可根据的大小决定，增大时，可减小k21；

k23是根据伺服阀的指令值|u|的大小决定的，当主动液压缸2需要调整时,控制器根据计算出的控制指令输出值u给伺服阀的驱动电路输出控制电压，而当|u|增大时，我们增大第二减振器10的阻尼，稳住摆杆9，摆杆9对主动液压缸2的运动的起到很好支撑作用，因此当|u|增大时可以增大k23，增加摆杆9的支撑刚度。

在上述实施过程中，通常二位二通的比例流量阀指令电压0～10v,对应阀芯开度0％～100％(即指令值越大，阀芯开度越大，阻尼越小)，因此当ul1、ul2的计算值大于10v时，则令ul1＝10v、ul2＝10v。

通常主动液压缸用的三位四通电液伺服阀或者电液比例阀指令电压-10v～+10v，对应阀芯开度-100％～100％(即指令电压的正负决定主动液压缸做伸长或收缩运动，指令电压绝对值的大小决定主动液压缸活塞杆行程的长短)，因此当u的计算值大于10v时，则令u＝10v；当u的计算值小于-10v，则令u＝-10v。

悬架控制系统软件需要实现喷杆上传感器信号的采集、数据处理、控制算法的执行、液压阀控制指令的输出以及工作参数输入和系统工作状态显示等功能。控制系统主程序的基本流程如图4所示，过程为：

系统启动后，开始自动初始化，等待人机界面发送指令，设置工作模式、工作参数等，下位机收到启动命令后，读取参数，并判断系统参数是否正常，然后发送给相应的处理模块。如果使用手动模式，用户通过触摸屏上的滑动条输入指定角度，下位机控制器输出控制指令给伺服阀，伺服阀控制主动液压缸2动作，调节喷杆角度到指定位置，手动模式调节算法采用pi比例积分算法。

如果使用自动控制模式，通过触摸屏的按钮启动下位机控制程序，控制程序启动定时器中断，定时器周期设置为5ms，以5ms为周期执行图3中程序的循环结构，进入中断响应程序后，采集超声波测距传感器、动态倾角传感器的数据；然后对采集的信号进行滤波处理和数据融合，相关算法见后文所述；处理后的角度反馈信号与设定的死区值进行比较；如果反馈信号不在死区范围内，调用发明设计的前馈补偿运动控制算法，根据反馈信号计算出控制指令信号，调用d/a数模转换子程序，根据伺服阀、第一比例流量阀、第二比例流量阀所在的控制通道，将数字信号转换为驱动模拟量信号，发送给各个阀组。伺服阀接收到控制指令以后，调整伺服阀的方向和开度，驱动主动液压缸2运动；同时，比例流量阀接收到控制指令以后，调节开度，实时调整两个减振器的阻尼系数；如果反馈信号值在设置的死区范围内，将输出的控制指令信号置0。

上述过程中，对各传感器反馈信号的处理算法概括如下：

第一测距传感器5检测喷杆左侧测点距离地面的高度h1，第二测距传感器7检测喷杆右侧测点距离地面的高度h2。首先对两组高度信号h1和h2进行五点滑动平均滤波；然后进行限幅处理，避免控制系统的错误响应，导致喷杆触碰到作物或地面；然后换算出喷杆相对地面的角度

定义δβ＝(β2-β1)，如果β3＞pg，且则使用数据融合算法，即优先使用最优加权算法将β3与喷杆相对支撑架的角度(β2-β1)进行数据融合，则使用数据融合后的βd作为有效输入角度反馈给控制器。为δβ的一阶导数，即喷杆相对支撑架转动的角速度。

如果角速度的绝对值则使用β3作为有效输入角度反馈给控制器。

pg和pf表示是否启用数据融合的分割系数，将通过试验多次调试来获取最佳值。

压力传感器、侧倾角传感器的数据在采集以后都使用10hz一下的巴特沃斯低通滤波器进行处理，然后再将数据反馈给下位机控制器。

上述试验，可通过以下过程完成：

(1)选择一块种植作物的条田，作物的高度最好在20-80厘米；

(2)喷雾机开到田头，车轮对准行间距，展开喷杆，调至水平状态；

(3)设置特殊工况两种，第一种工况是在车轮行进前方，一侧车轮行驶的地面上布置20厘米的凸台；第二中工况是在车轮行进前方，找一处有作物倒伏的地块，使喷杆一侧的超声波传感器从倒伏作物上方经过；

(4)将内置有陀螺仪的动态倾角传感器，安装在喷杆的中心位置，测量喷杆的滚转角加速度和垂直加速度；

(5)设定pg等于0.1，pf的设定值从0.1开始试验，依次经过两种试验工况测试，分别记录喷杆的角加速度值和垂直加速度；

(6)依次增大pf，开展多组试验，直到喷杆的角加速度值和垂直加速度的加权平均值最小，记录pg和pf的数值；

(7)依次设定pg等于0.2,0.3,...5，重复步骤(5)和步骤(6)。

(8)选择试验组中喷杆角加速度值和垂直加速度的加权平均值最小的一组,即最优参数。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，本发明要求保护范围由所附的权利要求书、说明书及其等效物界定。