一种随动系统俯仰角度跟踪控制方法与流程

本发明属于随动控制技术领域，具体涉及一种随动系统的俯仰角度跟踪算法。

背景技术：

随动系统是武器系统的重要组成部分，它是构成武器自动化体系的基本环节。武器跟踪随动系统能够为火炮、导弹武器提供360°方位角和-10°～+70°射向内的目标跟踪。随动控制系统能够根据外界输入的控制量的变化，实时控制驱动执行设备精确跟踪外部指令。

随动系统装置具有回转和俯仰两个自由度，主要由基座、回转机、转塔、俯仰机和发射架组成。

随动控制系统接收到上位系统的目标角度信息，根据方位测角传感器和俯仰测角传感器采集到的回转机与俯仰机当前角度信息，采用比例-积分-微分(pid)控制算法，分别输出速度指令给回转驱动器、俯仰驱动器，伺服驱动器采用速度控制模式，根据速度指令驱动电机转动，从而实现转台回转运动和发射架俯仰运动，进而实现目标跟踪。传统随动系统俯仰控制框图如图1所示。

转塔回转角度与回转电机转动角度成正比线性关系，采用pid控制算法能够实现快速、高精度的跟踪控制；而转台俯仰角度与俯仰电机转动角度是三角函数关系，不是线性关系，而pid控制算法本身是线性控制算法，因此采用pid控制算法无法实现快速、高精度跟踪控制。因此，研究一种新的随动系统俯仰控制算法具有重要意义。

技术实现要素：

本发明要解决的技术问题是：提供一种随动系统俯仰俯仰角度跟踪控制方法，即一种随动控制算法，实现武器跟踪系统俯仰机的快速、高精度随动控制。

为了解决上述技术问题，本发明提出随动系统俯仰俯仰角度跟踪控制方法，随动控制系统比较俯仰给定角度值和角度反馈值计算角度误差，使用前馈+比例微分(pd)控制算法，输出俯仰速度给定值，使用速度转换方程将俯仰角速度给定值转换为电机速度给定值，输出至电机驱动器，实现pid控制线性化，驱动器进行速度控制驱动电机转动，带动电作动筒伸出收回，从而实现俯仰机构的快速、高精度目标跟踪控制。

本发明的技术方案是：一种随动系统俯仰角度跟踪控制方法，其特征在于，该方法包括下述内容：

比较俯仰给定角度值和俯仰角度反馈值θp，计算角度误差，使用前馈加比例微分控制算法，输出俯仰速度给定值采用速度转换方程ωm^*＝f(θp)·ωp^*，将俯仰角速度给定值转换为电机速度给定值输出至电机驱动器；

其中前馈加比例微分控制算法如下：

其中，kp为比列增益，kd为微分增益，kf前馈增益；

采用速度转换方程为：

俯仰机的电机速度与俯仰角速度的关系为ωm^*＝f(θp)·ωp^*

其中，d为电作动筒丝杆导程，r为电机和电作动筒的齿轮总减速比；

进行俯仰运动的俯仰机的上支架的长度为loa、进行俯仰运动的俯仰机的下支架的长度为lob；俯仰角θp；θ0为上支架和下支架的初始夹角。

本发明的技术效果是：实现pid控制线性化，驱动器进行速度控制驱动电机转动，带动电作动筒伸出收回，从而实现俯仰机构的快速、高精度目标跟踪控制。

附图说明

图1传统俯仰控制框图；

图2俯仰简化结构示意图；(a)水平状态；(b)起竖状态。

图3本发明的(改进)俯仰控制框图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施案例，对本发明的具体实施方式进一步详细描述。

一种随动系统俯仰角度跟踪控制方法，其特征在于，该方法包括下述内容：

比较俯仰给定角度值和俯仰角度反馈值θp，计算角度误差，使用前馈加比例微分控制算法，输出俯仰速度给定值采用速度转换方程ωm^*＝f(θp)·ωp^*，将俯仰角速度给定值转换为电机速度给定值输出至电机驱动器；

其中前馈加比例微分控制算法如下：

其中，kp为比列增益，kd为微分增益，kf前馈增益；

采用速度转换方程为：

俯仰机的电机速度与俯仰角速度的关系为ωm＝f(θp)·ωp

其中，d为电作动筒丝杆导程，r为电机和电作动筒的齿轮总减速比；

进行俯仰运动的俯仰机的上支架的长度为loa、进行俯仰运动的俯仰机的下支架的长度为lob；俯仰角θp；θ0为上支架和下支架的初始夹角。

另外，俯仰机的俯仰结构简化为：具有俯仰轴o、上支点a、下支点b三点，组成上支架oa、下支架ob、电作动筒ab三边的三角形，三点俯仰轴o、上支点a、下支点b位置和上支架、下支架长度都是固定不动的；俯仰角为电作动筒长度变化时，变化的上支架、下支架之间的角度。

采用多项式方程公式(6)替代公式(5)

多项式方程系数a0，a1，a2，…可采用曲线拟合的方式求取。

另外，首先在俯仰机构角度范围内，对俯仰角度等距取点计算的函数值y＝f(θp)，再使用matlab的plotfit函数或excel工具进行2阶多项式曲线拟合，求取多项式系数a0，a1，a2。

另外，增大多项式方程阶次，再次进行曲线拟合计算a0，a1，a2之外的多项式系数。

本发明的原理是：

俯仰机是随动转台的主要功能结构，用于实现随动系统的俯仰运动，由电机、电作动筒、驱动器、高低测角传感器、上支架、下支架组成。俯仰机通过将电作动筒前、后两端分别联接在发射架和转塔安装座上，形成由电作动筒、转塔、发射架共同构成的摇杆滑块机构，电作动筒伸缩带动发射架实现俯仰运动。

图1为其俯仰控制框图。

其中(随动系统进行俯仰运动的俯仰机包括电作动筒、上支架、下支架。

俯仰简化结构如图2所示，由俯仰轴o、上支点a、下支点b三点和上支架oa、下支架ob、电作动筒ab三边组成，形成一个三角形，三点位置和上支架、下支架长度都是固定不动的，只有电作动筒长度可变化；aob夹角∠aob＝θ0+θp，俯仰角θp＝0时，aob夹角值为θ0，对应的电作动筒长度lab0，作为电作动筒初始长度值。电机转动角度θm变化，通过齿轮、丝杆带动电作动筒长度lab变化，使得俯仰角θp变化。

根据余弦定理，电作动筒长度lab与俯仰角θp关系如下：

俯仰电作动筒长度lab与电机转动角度θm关系如下：

其中，d为电作动筒丝杆导程，r为电机和电作动筒的齿轮总减速比。

将公式2带入公式1，可得

对上式进行求导，可得

由上式可得，电机速度ωm与俯仰角速度ωp的关系ωm＝f(θp)ωp

由式3可知，俯仰角度θp与俯仰电机转动θm角度是三角函数关系，不是线性关系，而pid控制算法本身是线性控制算法，因此只采用pid控制算法无法实现快速、高精度跟踪控制。

本发明对传统俯仰pid控制算法进行了改进，控制系统比较俯仰给定角度值和俯仰角度反馈值θp，计算角度误差，使用前馈+比例微分(pd)控制算法，输出俯仰速度给定值并增加了速度转换方程ωm^*＝f(θp)·ωp^*，将俯仰角速度给定值转换为电机速度给定值输出至电机驱动器，实现pid控制线性化，驱动器进行速度控制驱动电机转动，带动电作动筒伸出收回，从而实现俯仰机构的快速、高精度目标跟踪控制，控制框图如图3所示。使用前馈+pd控制比pid控制算法的跟踪误差更小，超调更小，前馈+pd控制算法如下：

其中，kp为比列增益，kd为微分增益，kf前馈增益。

随动控制系统一般使用dsp或单片机等嵌入式处理器，公式5涉及开平方、正余弦函数计算，计算量比较大，软件运算周期比较长，不适合嵌入式控制。多项式方程计算简单，运算量小，本发明采用多项式方程(公式6)替代公式(5)

多项式方程系数a0，a1，a2，…可采用曲线拟合的方式求取。

首先在俯仰机构角度范围内，对俯仰角度等距取点(例如每隔0.5°取一点)计算的函数值y＝f(θp)，再使用matlab的plotfit函数或excel等工具进行2阶多项式曲线拟合，求取多项式系数a0，a1，a2；根据曲线拟合误差情况决定是否增大多项式方程阶次，若需阶次增大则再次进行曲线拟合计算多项式系数和误差，一般多项式方程阶次取2～3阶即可满足使用要求。