CN112327610B - 一种双钟摆喷杆悬架的建模和位置跟踪的控制方法 - Google Patents

Abstract

一种双钟摆喷杆悬架的建模和位置跟踪的控制方法，第一步：传感器检测喷杆和路面之间的倾斜角度，主控制器控制方法为读取喷杆和路面之间的倾斜角度信号与主控制器给定的控制命令信号，通过作差计算得出误差信号；第二步：根据第一步中的误差信号，主控制器按照预定的控制算法计算出的喷杆的高度以及驱动伺服阀的电流信号的控制量，传递给伺服阀驱动单元，通过伺服阀驱动单元对植保机械喷杆进行角度调节并按照预定高度进行工作，实现对喷杆和路面之间的倾斜角度或植株冠层高度的跟踪。本发明使喷杆悬架在具有干扰的影响和作用下依旧能够保持较好稳定性的能力，从而保证施药的均匀性。对喷杆的位姿控制研究以及农业施药技术的发展具有重要的意义。

Description

一种双钟摆喷杆悬架的建模和位置跟踪的控制方法

技术领域：

本发明涉及智能农业植保机械的控制领域，尤其是关于用于田间工作的大型植保机械的双钟摆喷杆悬架的建模和控制方法。

背景技术：

为了解决病虫草害等农业问题，植保机械对于经济作物高产、面粮稳产上逐渐发挥着不可替代的作用，目前已经在世界上许多国家的植保机械中被广泛研究与使用。大型植保机械的喷杆悬架，主要有双连杆、梯形悬架、摆形悬架等等结构。到目前为止，国内外学者得到了喷杆悬架的频响函数，但是如今并没有对喷杆悬架状态空间形式的研究，除此之外，大多数对植保机械的液压驱动控制器采用较为传统的经典控制策略，然而，在田间的实际长时间连续工作过程中，植保机械极易受到外界环境、传感器特性变化等因素的影响，一些控制策略难以满足对喷杆的角度的控制需求，

发明内容：

发明目的：

为了解决上述问题，本发明提供了一种双钟摆喷杆悬架的建模方法以及一种跟踪控制器的设计方法。

技术方案：

一种植保机械双钟摆喷杆悬架的跟踪控制方法，其特征在于：所述方法步骤如下：

第一步：传感器检测喷杆和路面之间的倾斜角度，主控制器控制方法为读取喷杆和路面之间的倾斜角度信号与主控制器给定的控制命令信号(设定的目标值，根据传感器实时测量得到反馈信号，然后通过控制器作用于执行器上，从而驱动伺服阀带动喷杆的动作)，通过作差计算得出误差信号；

第二步：根据第一步中的误差信号，主控制器按照预定的控制算法计算出的喷杆的高度(是喷杆两侧和地面间的高度。因为在实现跟踪(即喷杆平行于地面)时最终喷杆两侧距离地面的高度是相同的)以及驱动伺服阀的电流信号的控制量，传递给伺服阀驱动单元，通过伺服阀驱动单元对植保机械喷杆进行角度调节并按照预定高度进行工作，实现对喷杆和路面之间的倾斜角度或植株冠层高度的跟踪。

主控制器构建方法如下：

1)、结合第二类Lagrange(拉格朗日)动力学方程(

即为第二类Lagrange方程)，建立植保机双钟摆喷杆悬架的非线性模型，并在工作点处线性化得到的双钟摆喷杆悬架的状态方程；

2)、基于所得到的双钟摆喷杆悬架的状态方程，引用一定的不确定参数(这个参数是考虑到了植保机械在工作过程中温度、湿度等环境因素对传感器造成的影响以及元器件老化等因素，悬架的状态方程会发生一定的变化，因此考虑了引入不确定参数。)，选取线性二次型性能指标(最优控制中的线性二次型性能指标)，构建主控制器。通过主控制器实时控制喷杆的位姿，从而使之对地面倾角或作物冠层高度实现渐进跟踪，实现仿形控制。

1)步骤中结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型,并在工作点处线性化得到的双钟摆喷杆悬架的状态方程的步骤如下：

第二类Lagrange动力学方程为：

其中L＝T-V为拉格朗日函数，T为植保机械的总动能，V为植保机械总势能，D_L为所消耗与散失的热能，Q_i(i＝1,2,3...)为广义坐标θ_i(i＝1,2,3...)下的广义力。

喷杆悬架中总动能T主要包括平动动能T_t和喷杆的转动动能T_r：

T＝T_t+T_r (2)

L₁和L₂分别为一、二级摆杆的摆长，设喷杆重心坐标为B(x_B，y_B)，x_B为重心的横坐标， y_B为纵坐标，则有：

B点的速度为：

此公式为喷杆重心点移动速度的计算，也是对上式位置变化公式的进一步求导；

M为喷杆的质量，因此，喷杆的平动动能T_t为：

I为喷杆的转动惯量，则喷杆的转动动能T_r可以表示为：

此处根据动能定理计算喷杆的转动动能；

因此，总动能T(平动转动动能之和)为：

喷杆在运动过程中只受到重力的作用，因此，喷杆所具有的重力势能V为：

V＝Mg(L₁ cosθ₁+L₂ cosθ₂) (8)

g为公知的重力加速度；

因此由第二类Lagrange动力学方程可得：

C为阻尼器的阻尼系数，因此，经整理可得到Rayleigh耗散函数D_L：

其中，Q_i为广义坐标下的广义力，其中Q₁＝0，Q₂＝F，其中F为液压驱动施加给喷杆的广义力矩。

因此可得到以下两个非线性运动方程：(这个方程是双钟摆喷杆悬架系统在广义坐标下建系并采用第二类Lagrange方程所得到的非线性运动学方程，植保机双钟摆喷杆悬架的非线性模型)

针对大型植保机械的双钟摆喷杆悬架具有高度的非线性特性，在得到非线性运动方程之后，考虑到悬架位置一般处于工作点附近，选取状态向量为

在初始工作点的线性化后，可以表示为：

因此，可将非线性方程线性化表示为状态空间的形式：

令：

其中，A＝a^-1b，B＝a^-1c，C＝c^T，u为控制律，A为状态矩阵，B为控制矩阵，C为输出矩阵。

因此，可以得到状态空间方程：

将二级摆杆和竖直方向的夹角θ₂作为输出，则输出方程为(这个式子是系统模型的输出，为二级摆杆和竖直方向的夹角，不是角度的控制量。是控制量作用后角度的变化(输出))：

针对大型植保机械的双钟摆喷杆悬架跟踪控制器的设计目标，在得到了状态空间方程之后，双钟摆喷杆悬架跟踪控制器的设计目标是使输出角度y(t)用于跟踪角度的期望输出角度y_r，(最终的控制量应该是按照此方法经过计算求得反馈系数K作为反馈控制器参数，最终的控制信号为此系数与所选取的状态变量当前状态的乘积，控制信号是由状态和反馈系数共同决定的)因此，定义跟踪误差向量为：

双钟摆喷杆悬架的跟踪控制器的设计，需要建立一个新的增广方程，由于本发明目的是通过控制二级摆杆的摆角从而使喷杆角度跟踪对路面的实时倾角实现跟踪，可以得到增广方程：

据此，可将双钟摆喷杆悬架的状态空间方程改写成一个新的方程的形式：

其中：

其中，x(t)∈Rⁿ，为状态向量u(t)∈R^m，为控制向量，y(t)∈R^p，为输出向量，A、B、 C分别是根据模型而具有的适当维数的标称矩阵，矩阵ΔA是具有不确定性的矩阵，维数和矩阵A 相同。同时不确定性矩阵ΔA主要取决于不确定性向量α，ΔA的描述方式会影响讨论问题的复杂程度，因此，本发明采用范数有界形式描述：

ΔA＝DF(t)E (22)

其中D和E是具有适当维数的实常数矩阵，F(t)为有界的实矩阵函数，其元素是Lebesgue 可测的，并且满足F^T(t)F(t)≤I。在这里，ΔA＝DF(t)E，并有F(t)^TF(t)≤I，矩阵F(t)受不确定参数α₁、α₂、α₃、α₄的影响，并且各个不确定量的取值分别为0≤α₁≤1、0≤α₂≤1、0≤α₃≤1、0≤α₄≤1，I代表单位矩阵。ΔA矩阵表示的是权利要求4所建立的数学模型中的参数摄动，即受到温度、工况变化等等影响产生的系统参数不确定变化。

在这里，性能指标选取：

选择Lyapunov函数如下所示：

V(z,t)＝z^TPz (24)

对该Lyapunov函数求导，得到：

将

带入式(25)中，有：

而对于2z^T(ΔA_z)^TPz，取ε＝1，可得：

两次应用Schur补引理，可得到关于P线性矩阵不等式：

综上，稳定性证明完毕。

同时，满足ΔA＝D_zF(t)E_z，

E_z＝[E 0]。

P为线性矩阵不等式的解，K为状态反馈控制器参数，最终的控制信号为u＝-Kx(t)

这个部分是稳定性的一个证明，过程是根据想要达到的性能指标，构造李雅普诺夫函数证明系统稳定性，并通过相关的引理、定理的等价变换，逐步推导出满足性能指标的控制器参数所成立的充分条件，即关于P的线性矩阵不等式。只有满足这一条件，再带入系统参数求解出矩阵不等式，得到的控制器参数才是满足需求的、有意义的。并不仅仅是一个验证过程，也是求解控制器参数的重要过程。

针对大型植保机械的双钟摆喷杆悬架，采用CAN总线串行通讯协议传输数据，利用STM32微处理器对植保机械喷杆位置控制。其特征在于：以STM32微处理器为主控制器，主控制器的输入接高度检测模块、输出接电机驱动模块；CAN总线通讯模块；以及电源供电模块。

针对大型植保机械的双钟摆喷杆悬架，基于STM32微处理器将输出信号提供驱动单元，实现了对双钟摆喷杆悬架位姿的实时控制。

该双钟摆喷杆悬架为大型植保机械的重要组成部分，基于其机理结构，采用第二类Lagrange坐标系下建立并得到了双钟摆喷杆悬架的非线性方程，基于所得到的状态空间方程，考虑到不确定性并采用一种控制策略设计跟踪控制器。使喷杆位置能快速地实现对冠层的高度跟踪。步骤如下：

1)考虑到植保机械双钟摆喷杆悬架的工作原理和结构特征，结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型，并在工作点处线性化得到的状态空间方程；

2)设计跟踪控制器，基于所得到的双钟摆喷杆悬架的状态方程，引用一定的不确定参数，选取了合适的性能指标，实时控制喷杆的位姿，从而使之对地面倾角或作物冠层高度实现渐进跟踪，实现仿形控制

优点效果：

本发明采用第二类Lagrange动力学方程对双钟摆喷杆悬架进行了建模，得到了双钟摆悬架的状态空间模型，并基于此考虑到了一定的不确定性设计了一种跟踪控制器，使喷杆悬架在具有干扰的影响和作用下依旧能够保持较好稳定性的能力，从而保证施药的均匀性。对喷杆的位姿控制研究以及农业施药技术的发展具有重要的意义。

附图说明：

图1为直流稳压电源；

图2为液压驱动信号输出；

图3为角度传感器信号输入；

图4为控制器主芯片；

具体实施方式

一种植保机械双钟摆喷杆悬架的跟踪控制方法，其特征在于：所述方法步骤如下：

第一步：传感器检测喷杆和路面之间的倾斜角度，主控制器控制方法为读取喷杆和路面之间的倾斜角度信号与主控制器给定的控制命令信号(设定的目标值，根据传感器实时测量得到反馈信号，然后通过控制器作用于执行器上，从而驱动伺服阀带动喷杆的动作)，通过作差计算得出误差信号；

第二步：根据第一步中的误差信号，主控制器按照预定的控制算法计算出的喷杆的高度(是喷杆两侧和地面间的高度。因为在实现跟踪(即喷杆平行于地面)时最终喷杆两侧距离地面的高度是相同的)以及驱动伺服阀的电流信号的控制量，传递给伺服阀驱动单元，通过伺服阀驱动单元对植保机械喷杆进行角度调节并按照预定高度进行工作，实现对喷杆和路面之间的倾斜角度或植株冠层高度的跟踪。

主控制器构建方法如下：

1)、结合第二类Lagrange(拉格朗日)动力学方程(

即为第二类Lagrange方程)，建立植保机双钟摆喷杆悬架的非线性模型，并在工作点处线性化得到的双钟摆喷杆悬架的状态方程；

2)、基于所得到的双钟摆喷杆悬架的状态方程，引用一定的不确定参数(这个参数是考虑到了植保机械在工作过程中温度、湿度等环境因素对传感器造成的影响以及元器件老化等因素，悬架的状态方程会发生一定的变化，因此考虑了引入不确定参数。)，选取线性二次型性能指标(最优控制中的线性二次型性能指标)，构建主控制器。通过主控制器实时控制喷杆的位姿，从而使之对地面倾角或作物冠层高度实现渐进跟踪，实现仿形控制。

1)步骤中结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型,并在工作点处线性化得到的双钟摆喷杆悬架的状态方程的步骤如下：

第二类Lagrange动力学方程为：

其中L＝T-V为拉格朗日函数，T为植保机械的总动能，V为植保机械总势能，D_L为所消耗与散失的热能，Q_i(i＝1,2,3...)为广义坐标θ_i(i＝1,2,3...)下的广义力。

喷杆悬架中总动能T主要包括平动动能T_t和喷杆的转动动能T_r：

T＝T_t+T_r (2)

L₁和L₂分别为一、二级摆杆的摆长，设喷杆重心坐标为B(x_B，y_B)，则有：

B点的速度为：

此公式为喷杆重心点移动速度的计算，也是对上式位置变化公式的进一步求导；

M为喷杆的质量，因此，喷杆的平动动能T_t为：

I为喷杆的转动惯量，则喷杆的转动动能T_r可以表示为：

此处根据动能定理计算喷杆的转动动能；

因此，总动能T(平动转动动能之和)为：

喷杆在运动过程中只受到重力的作用，因此，喷杆所具有的重力势能V为：

V＝Mg(L₁ cosθ₁+L₂ cosθ₂) (8)

g为公知的重力加速度；

因此由第二类Lagrange动力学方程可得：

C为阻尼器的阻尼系数，因此，经整理可得到Rayleigh耗散函数D_L：

其中，Q_i为广义坐标下的广义力，其中Q₁＝0，Q₂＝F，其中F为液压驱动施加给喷杆的广义力矩。

因此可得到以下两个非线性运动方程：(这个方程是双钟摆喷杆悬架系统在广义坐标下建系并采用第二类Lagrange方程所得到的非线性运动学方程，植保机双钟摆喷杆悬架的非线性模型)

针对大型植保机械的双钟摆喷杆悬架具有高度的非线性特性，在得到非线性运动方程之后，考虑到悬架位置一般处于工作点附近，选取状态向量为

在初始工作点的线性化后，可以表示为：

因此，可将非线性方程线性化表示为状态空间的形式：

令：

其中，A＝a^-1b，B＝a^-1c，C＝c^T，u为控制律，A为状态矩阵，B为控制矩阵，C为输出矩阵。

因此，可以得到状态空间方程：

将二级摆杆和竖直方向的夹角θ₂作为输出，则输出方程为(这个式子是系统模型的输出，为二级摆杆和竖直方向的夹角，不是角度的控制量。是控制量作用后角度的变化(输出))：

针对大型植保机械的双钟摆喷杆悬架跟踪控制器的设计目标，在得到了状态空间方程之后，双钟摆喷杆悬架跟踪控制器的设计目标是使输出角度y(t)用于跟踪角度的期望输出角度y_r，(最终的控制量应该是按照此方法经过计算求得反馈系数K作为反馈控制器参数，最终的控制信号为此系数与所选取的状态变量当前状态的乘积，控制信号是由状态和反馈系数共同决定的)因此，定义跟踪误差向量为：

双钟摆喷杆悬架的跟踪控制器的设计，需要建立一个新的增广方程，由于本发明目的是通过控制二级摆杆的摆角从而使喷杆角度跟踪对路面的实时倾角实现跟踪，可以得到增广方程：

据此，可将双钟摆喷杆悬架的状态空间方程改写成一个新的方程的形式：

其中：

其中，x(t)∈Rⁿ，为状态向量u(t)∈R^m，为控制向量，y(t)∈R^p，为输出向量，A、B、 C分别是根据模型而具有的适当维数的标称矩阵，矩阵ΔA是具有不确定性的矩阵，维数和矩阵A 相同。同时不确定性矩阵ΔA主要取决于不确定性向量α，ΔA的描述方式会影响讨论问题的复杂程度，因此，本发明采用范数有界形式描述：

ΔA＝DF(t)E (22)

其中D和E是具有适当维数的实常数矩阵，F(t)为有界的实矩阵函数，其元素是Lebesgue 可测的，并且满足F^T(t)F(t)≤I。在这里，ΔA＝DF(t)E，并有F(t)^TF(t)≤I，矩阵F(t)受不确定参数α₁、α₂、α₃、α₄的影响，并且各个不确定量的取值分别为0≤α₁≤1、0≤α₂≤1、0≤α₃≤1、0≤α₄≤1，I代表单位矩阵。ΔA矩阵表示的是权利要求4所建立的数学模型中的参数摄动，即受到温度、工况变化等等影响产生的系统参数不确定变化。

在这里，性能指标选取：

选择Lyapunov函数如下所示：

V(z,t)＝z^TPz (24)

对该Lyapunov函数求导，得到：

将

带入式(25)中，有：

而对于2z^T(ΔA_z)^TPz，取ε＝1，可得：

两次应用Schur补引理，可得到关于P线性矩阵不等式：

综上，稳定性证明完毕。

同时，满足ΔA＝D_zF(t)E_z，

E_z＝[E 0]。

P为线性矩阵不等式的解，K为状态反馈控制器参数，最终的控制信号为u＝-Kx(t)

这个部分是稳定性的一个证明，过程是根据想要达到的性能指标，构造李雅普诺夫函数证明系统稳定性，并通过相关的引理、定理的等价变换，逐步推导出满足性能指标的控制器参数所成立的充分条件，即关于P的线性矩阵不等式。只有满足这一条件，再带入系统参数求解出矩阵不等式，得到的控制器参数才是满足需求的、有意义的。并不仅仅是一个验证过程，也是求解控制器参数的重要过程。

针对大型植保机械的双钟摆喷杆悬架，采用CAN总线串行通讯协议传输数据，利用STM32微处理器对植保机械喷杆位置控制。其特征在于：以STM32微处理器为主控制器，主控制器的输入接高度检测模块、输出接电机驱动模块；CAN总线通讯模块；以及电源供电模块。

针对大型植保机械的双钟摆喷杆悬架，基于STM32微处理器将输出信号提供驱动单元，实现了对双钟摆喷杆悬架位姿的实时控制。其特征在于：

该双钟摆喷杆悬架为大型植保机械的重要组成部分，基于其机理结构，采用第二类Lagrange坐标系下建立并得到了双钟摆喷杆悬架的非线性方程，基于所得到的状态空间方程，考虑到不确定性并采用一种控制策略设计跟踪控制器。使喷杆位置能快速地实现对冠层的高度跟踪。步骤如下：

1)考虑到植保机械双钟摆喷杆悬架的工作原理和结构特征，结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型，并在工作点处线性化得到的状态空间方程；

2)设计跟踪控制器，基于所得到的双钟摆喷杆悬架的状态方程，引用一定的不确定参数，选取了合适的性能指标，实时控制喷杆的位姿，从而使之对地面倾角或作物冠层高度实现渐进跟踪，实现仿形控制

下面对本发明做进一步详细说明：

一种双钟摆喷杆悬架的建模方法

1)基于双钟摆喷杆悬架结构，采用第二类Lagrange动力学方程，进行建模，得到非线性方程，将模型在其工作点处线性化得到的状态空间形式；

2)设计跟踪控制器，基于对双钟摆喷杆悬架建模所得到状态方程，通过调节液压缸长度对喷杆悬架进行控制，使植保机械的喷杆能够和路面或植物冠层的倾角保持平行，并实现对角度的渐近跟踪。

步骤1)第二类Lagrange动力学方程为：

其中L＝T-V为拉格朗日函数，T为双钟摆喷杆悬架所具有的总动能，V为双钟摆喷杆悬架所具有的总势能，D_L为阻尼器所消耗与散失的热能，Q_i(i＝1,2,3...)为广义坐标θ_i(i＝1,2,3...)下的广义力。

双钟摆喷杆悬架中的一级摆杆和二级摆杆的质量可忽略不计，喷杆所具有的总动能T主要包括两部分：喷杆的平动动能T_t和喷杆的转动动能T_r：

T＝T_t+T_r (2)

L₁和L₂分别为一、二级摆杆的摆长，设喷杆重心坐标为B(x_B，y_B)，θ₁、θ₂分别为一二级摆杆和竖直方向的夹角，则有：

B点的速度为：

M为喷杆的质量，因此喷杆的平动动能T_t为：

I为喷杆的转动惯量，则喷杆的转动动能T_r可以表示为：

因此，所具有的总动能T为：

喷杆在运动过程中只受到重力的作用，因此，喷杆所具有的重力势能V为：

V＝Mg(L₁ cosθ₁+L₂ cosθ₂) (8)

因此由第二类Lagrange动力学方程可得：

因此，可得到Rayleigh耗散函数D_L，其中C为阻尼器的阻尼系数：

因此可得到以下两个非线性运动学方程：

其中F为液压驱动施加给喷杆的广义力矩。

步骤2)在得到两个非线性运动学方程之后，考虑到双钟摆喷杆悬架具有高度的非线性特性因此控制器很难进行设计，同时考虑到悬架位置一般处于工作点附近，所以本发明对建模后的模型在初始状态进行线性化。

选取状态向量为

在初始工作点处线性化后可以表示为：

因此，可线性化并表示为状态空间的形式：

令：

其中，A＝a^-1b，B＝a^-1c，C＝c^T，u表示控制律，A为的状态矩阵，B为控制矩阵，C 为输出矩阵。

因此，可以得到状态空间方程：

将二级摆杆和竖直方向的夹角θ₂作为输出，则输出方程为：

步骤3)得到了系统的状态空间形式之后，双钟摆喷杆悬架跟踪控制器的设计目标是使输出角度 y(t)用于跟踪角度的期望输出角度y_r，定义跟踪误差向量为：

双钟摆喷杆悬架的跟踪控制器的设计，需要建立一个新的增广方程，由于本发明目的是通过控制二级摆杆的摆角从而使喷杆角度跟踪对路面的实时倾角实现跟踪，可以得到增广方程：

据此，可将双钟摆喷杆悬架的状态空间方程改写成一个新的方程的形式：

其中：

其中，x(t)∈Rⁿ，为状态向量u(t)∈R^m，为控制向量，y(t)∈R^p，为输出向量，A、B、 C分别是根据模型而具有的适当维数的标称矩阵，矩阵ΔA是具有不确定性的矩阵，维数和矩阵A 相同。同时不确定性矩阵ΔA主要取决于不确定性向量α，ΔA的描述方式会影响讨论问题的复杂程度，因此，本发明采用范数有界形式描述：

ΔA＝DF(t)E (22)

其中D和E是具有适当维数的实常数矩阵，F(t)为有界的实矩阵函数，其元素是Lebesgue 可测的，并且满足F^T(t)F(t)≤I。在这里，ΔA＝DF(t)E，并有F(t)^TF(t)≤I，矩阵F(t)受不确定参数α₁、α₂、α₃、α₄的影响，并且各个不确定量的取值分别为0≤α₁≤1、0≤α₂≤1、0≤α₃≤1、0≤α₄≤1。

在这里，性能指标选取：

证明：将控制律u＝Kz带入到增广状态空间方程中，可以得到：

选择Lyapunov函数如下所示：

V(z,t)＝z^TPz (25)

对该Lyapunov函数求导，得到：

因此：

而对于2z^T(ΔA_z)^TPz，取ε＝1，并经整理可得：

其中，

E_z＝[E 0]。

两次应用Schur补引理，可得到关于P线性矩阵不等式：

综上，稳定性证明完毕。

步骤4)采用CAN总线串行通讯协议传输数据，利用STM32微处理器对植保机械喷杆位置控制。其特征在于：以STM32微处理器为主控制器，主控制器的输入接高度检测模块、输出接电机驱动模块；CAN总线通讯模块；以及电源供电模块。

步骤5)基于STM32微处理器将输出信号提供驱动单元，实现了对双钟摆喷杆悬架的喷杆位置的实时控制。其特征在于：传感器检测路面的倾斜角度，主控制器读取高度检测单元的反馈信号与主控制器给定的控制命令信号，通过作差计算得出误差信号。根据误差信号，主控制器按照预定的控制算法计算出的喷杆的高度以及驱动伺服阀的电流信号的控制量，传递给伺服阀驱动单元，植保机械喷杆进行角度调节并按照预定高度进行工作，实现对路面倾角或植株冠层高度的跟踪。结论：

本发明解决了双钟摆喷杆悬架的状态空间模型的建立问题，同时解决了双钟摆悬架中的喷杆无法较好地跟踪路面倾角或冠层高度的问题，基于采用第二类Lagrange方程建立的模型，考虑了不确定性，设计了一种跟踪控制器，对喷杆的位姿实现有效地控制，从而进一步提升喷杆位置的稳定性以及农药的施药均匀性。现已进行了大量的仿真模拟实验，结果表明，在多种贴近植保机喷施作业状态的复杂环境和工况下，控制器依然保持良好的性能，可有效地实现对喷杆状态的控制。

Claims (1)

1.一种植保机械双钟摆喷杆悬架的跟踪控制方法，其特征在于：所述方法步骤如下：

第一步：检测喷杆和路面之间的倾斜角度，读取喷杆和路面之间的倾斜角度信号与主控制器给定的控制命令信号，通过作差计算得出误差信号；

第二步：根据第一步中的误差信号，主控制器按照预定的控制算法计算出的喷杆的高度以及驱动伺服阀的电流信号的控制量，传递给伺服阀驱动单元，通过伺服阀驱动单元对植保机械喷杆进行角度调节并按照预定高度进行工作，实现对喷杆和路面之间的倾斜角度或植株冠层高度的跟踪；

主控制器构建方法如下：

1)、结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型，并在工作点处线性化得到的双钟摆喷杆悬架的状态方程；

2)、基于所得到的双钟摆喷杆悬架的状态方程，引用不确定参数，选取线性二次型性能指标，构建主控制器；

1)步骤中结合第二类Lagrange动力学方程，建立植保机双钟摆喷杆悬架的非线性模型,并在工作点处线性化得到的双钟摆喷杆悬架的状态方程的步骤如下：

第二类Lagrange动力学方程为：

其中L＝T-V为拉格朗日函数，T为植保机械的总动能，V为植保机械总势能，D_L为所消耗与散失的热能，Q_i，i＝1,2,3...为广义坐标θ_i，i＝1,2,3...下的广义力；

喷杆悬架中总动能T主要包括平动动能T_t和喷杆的转动动能T_r：

T＝T_t+T_r (2)

L₁和L₂分别为一、二级摆杆的摆长，设喷杆重心坐标为B(x_B，y_B)，x_B为重心的横坐标，y_B为纵坐标，则有：

B点的速度为：

M为喷杆的质量，喷杆的平动动能T_t为：

I为喷杆的转动惯量，则喷杆的转动动能T_r表示为：

总动能T为：

喷杆在运动过程中只受到重力的作用，喷杆所具有的重力势能V为：

V＝Mg(L₁cosθ₁+L₂cosθ₂) (8)

由第二类Lagrange动力学方程得：

C为阻尼器的阻尼系数，经整理得到Rayleigh耗散函数D_L：

其中，Q_i为广义坐标下的广义力，其中Q₁＝0，Q₂＝F，其中F为液压驱动施加给喷杆的广义力矩；

得到以下两个非线性运动方程：

在得到非线性运动方程之后，考虑到悬架位置一般处于工作点附近，选取状态向量为

在初始工作点的线性化后，表示为：

将非线性方程线性化表示为状态空间的形式：

令：

其中，A＝a^-1b，B＝a^-1c，C＝c^T，u为控制律，A为状态矩阵，B为控制矩阵，C为输出矩阵；

得到状态空间方程：

将二级摆杆和竖直方向的夹角θ₂作为输出，则输出方程为：

双钟摆喷杆悬架跟踪控制器的设计目标是使输出角度y(t)用于跟踪角度的期望输出角度y_r，定义跟踪误差向量为：

双钟摆喷杆悬架的跟踪控制器的设计，需要建立一个新的增广方程，由于目的是通过控制二级摆杆的摆角从而使喷杆角度跟踪对路面的实时倾角实现跟踪，得到增广方程：

据此，将双钟摆喷杆悬架的状态空间方程改写成一个新的方程的形式：

其中：

其中，x(t)∈Rⁿ，为状态向量u(t)∈R^m，为控制向量，y(t)∈R^p，为输出向量，A、B、C分别是根据模型而具有的适当维数的标称矩阵，矩阵ΔA是具有不确定性的矩阵，维数和矩阵A相同；同时不确定性矩阵ΔA取决于不确定性向量α，ΔA的描述方式会影响讨论问题的复杂程度，采用范数有界形式描述：

ΔA＝DF(t)E (22)

其中D和E是具有适当维数的实常数矩阵，F(t)为有界的实矩阵函数，其元素是Lebesgue可测的，并且满足F^T(t)F(t)≤I；在这里，ΔA＝DF(t)E，并有F(t)^TF(t)≤I，矩阵F(t)受不确定参数α₁、α₂、α₃、α₄的影响，并且各个不确定量的取值分别为0≤α₁≤1、0≤α₂≤1、0≤α₃≤1、0≤α₄≤1，I代表单位矩阵；

在这里，性能指标选取：

选择Lyapunov函数如下所示：

V(z,t)＝z^TPz (24)

对该Lyapunov函数求导，得到：

将

带入式(25)中，有：

而对于2z^T(ΔA_z)^TPz，取ε＝1，得：

两次应用Schur补引理，得到关于P线性矩阵不等式：

综上，稳定性证明完毕；

同时，满足ΔA＝D_zF(t)E_z，

E_z＝[E 0]。

Images