CN107783422B - 采用捷联惯导的火炮瞄准稳定系统控制方法 - Google Patents

Abstract

本发明提供了一种采用捷联惯导的火炮瞄准稳定系统控制方法，首先采集捷联惯导陀螺组所测量的三轴角速率并转换至大地坐标系下的角速率；然后计算车体系下高低和方位调转角速率、炮塔俯仰和横滚角速率以及车体航向角速率；其次，接受稳定控制指令，采集位置反馈，大幅调转时控制驱动器工作于调速模式；进入稳定模式后进行位置、前馈控制，速率稳定、速率干扰补偿控制，控制驱动器工作于电流或力矩模式；当达到稳定精度后上报瞄准好。本发明控制精度高，节约了成本，提高了可靠性。

Description

采用捷联惯导的火炮瞄准稳定系统控制方法

技术领域

本发明属于火炮随动系统领域，主要涉及需要精确稳定、跟踪的火炮随动系统的控制方法。

背景技术

随着军事斗争的发展演变，新军事作战需求迫切需要压制武器具备行进间射击的能力，需要自行火炮能够遂行动对静、动对动的作战任务。那么火炮需要在瞄准稳定系统的控制下，一是克服车体由于行走的路面对车体造成的扰动；二是火炮要跟随火控的跟踪瞄准调转指令。此类功能在坦克武器或两栖突击炮的炮控系统早已得到实现。但是，坦克炮控系统采用了摇架组合陀螺组、车体/炮塔陀螺组共同完成火炮的稳定瞄准，并配备惯导系统，安装在车体上，用于行进间的惯性导航。其中，摇架组合陀螺组由两组双轴速率陀螺和两组单轴速率陀螺组成，用于测量坦克炮在空间的运动状态，完成坦克炮高低向和方位向伺服的自动和手动的稳定瞄准控制；车体/炮塔陀螺组为三个单轴速率陀螺组用于测量车体航向、炮塔的俯仰和横滚角速率，用于炮控系统的速率干扰前馈补偿，使炮控系统达到或优于高低向0.8mil和方位向1.5mil的稳定精度。但是，坦克炮控系统所配置的陀螺太多，信息冗余，成本太高，可靠性较低，高价值的捷联惯导没有很好地利用，此类传统的炮控系统方案，不适用于现代压制武器的某自行火炮武器系统中。

随着捷联惯导(SINS)的技术进步，其精度提高和成本降低，长期稳定性和抗冲击能力的大幅提高，将SINS安装在自行火炮的身管上直接测量大地坐标系的指向是现代自行火炮的标配。自行火炮的随动系统将SINS方向和姿态值作为随动系统的角度反馈，实现了火炮在大地坐标系下的直接控制，提高了火炮瞄准精度。但是，随动系统的控制坐标与SINS测量坐标不统一，造成控制系统的高低和方位控制系统存在耦合。对于驻停间进行发射的自行火炮采用如四元数、欧拉角法等坐标变换或串级控制来解决。但对于需要行进间的自行火炮需要采用既能抑制车体扰动，又能很好解决控制耦合问题的新方法。

发明内容

为了克服现有技术的不足，本发明提供一种采用捷联惯导的火炮瞄准稳定系统控制方法，保留传统火炮随动系统组成的侧角器，利用SINS、高低侧角器和方位侧角器，采用空间坐标变换得到炮塔的俯仰和横贯角速率，以及车体航向的角速率，稳定系统采用复合控制方法实现火炮的高精度稳定跟踪控制。本发明采用SINS作为火炮随动系统的空间角度和惯性空间角速率速度反馈器件，抑制车体姿态对火炮身管指向的扰动以及高低向和方位向随动的控制耦合，实现火炮的高精度稳定跟踪控制方法问题；利用SINS直接测量火炮身管指向，提高指向控制精度，又可以使自行火炮武器系统完成自主定位导航功能。

本发明解决其技术问题所采用的技术方案包括以下步骤：

(1)设定速度环控制补偿的控制步数k的初值为0，k逐一增加；设定速度环控制补偿的控制周期T_s＝1ms；

(2)采集捷联惯导陀螺组所测量的三轴角速率ω_p(k)＝[ω_px(k) ω_py(k) ω_pz(k)]^T，其中，ω_px(k)为x轴角速率；ω_py(k)为y轴角速率；ω_pz(k)为z轴角速率；

(3)计算身管在大地坐标下的航向、俯仰和横滚调转角速率

其中，ψ(j),θ(j),

为捷联惯导SINS输出的航向角、俯仰角和横滚角；j为位置环计算步数；

(4)采集方向侧角器和高低侧角器的测量值β_b(k),ε_b(k)；利用非线性观测器提取炮塔在车体上、火炮在炮塔上的调转角速率

其中

e₁(k),e₂(k)为观测误差，α，δ分别为fal函数的参数0.01≤α≤1，0.01≤δ≤1，β₁₁,β₁₂分别为方向测角器观测器的一阶、二阶增益，β₂₁,β₂₂分别为高低测角器观测器的一阶、二阶增益，z₁₁(k)、z₂₁(k)分别为β_b(k),ε_b(k)的估计值，z₁₂(k)、z₂₂(k)分别为

的估计值；

(5)计算炮塔的横滚角、俯仰和方位轴角速率ω_hx(k),ω_hy(k),ω_hz(k)：

(6)计算车体的航向角速率

(7)设定位置控制的位置环计算步数j的初值为0，j逐一增加，位置控制的控制周期为10ms；判定k值，若k为10的倍数时，执行第(8)步，否则跳转至第(18)步；

(8)接收捷联惯导的航向、姿态和横滚角ψ(j),θ(j),

(9)接收大地坐标下的火炮瞄准控制指令ψ_ref(j),θ_ref(j)，其中，ψ_ref(j),θ_ref(j)分别为航向角和俯仰角；

(10)判断方位调炮控制误差e_β(j)＝ψ_ref(j)-ψ(j)，高低调炮控制误差e_ε(j)＝θ_ref(j)-θ(j)，若调炮控制误差e_β(j)≥e_βmax或e_ε(j)≥e_εmax，转入步骤(11)；否则，转入步骤(14)，其中，e_βmax,e_εmax分别为方位和高低分段控制误差阈值；

(11)计算大幅调转下的方位随动伺服驱动器的速度控制指令

其中：k_eβ为根号e控制系数；u_maxsβ和u_minsβ为方位根号e控制器输出的上限和下限；

(12)计算大幅调转下的高低随动伺服驱动器的速度控制指令

其中：k_eε为根号e控制系数；u_maxsε和u_minsε为高低根号e控制器输出的上限和下限；

(13)设定驱动器工作在调速模式，通过CAN总线分别向方位和高低伺服驱动器发送速度控制指令

(14)计算方位和高低位置控制系统的前馈控制量u_fβ(j),u_fε(j)

其中，k_fβ,k_fε分别为方位和高低系统的前馈控制器系数，

和

分别为航向角速率和俯仰角速率；

(15)计算高低和方位干扰速度补偿量u_dβ(j),u_dε(j)：

其中，

分别为高低和方位随动的干扰速率；k_dβ1,k_dβ2,k_dβ3分别为方位补偿器的综合系数；k_dε1,k_dε2,k_dε3,分别为高低补偿器的综合系数；

(16)计算稳定条件下的方位随动伺服驱动器的速度控制指令

并令方位角度等效误差为e_sβ(j)＝e_β(j)；

u_psβ(j)＝K_psβe_sβ(j)

u_presatsβ(j)＝u_psβ(j)+u_isβ(j)+u_fβ(j)+u_dβ(j)

其中：u_psβ(j)为比例控制项；u_isβ(j)为积分控制项；u_imaxsβ和u_iminsβ为方位速度积分控制器输出的上限和下限，u_imaxsβ(j)＝max(0,u_maxsβ-u_psβ(j))，u_iminsβ(j)＝min(0,u_minsβ-u_psβ(j))；K_psβ为PI比例控制系数；K_isβ为积分系数，u_presatsβ(j)为方位速度控制量预饱和值；

(17)计算稳定条件下的高低随动伺服驱动器的速度控制指令

并令高低角度等效误差e_sε(j)＝e_ε(j)；

u_psε(j)＝K_psεe_sε(j)

u_presatsε(j)＝u_psε(j)+u_isε(j)+u_fε(j)+u_dε(j)

其中：u_psε(j)为比例控制项；u_isε(j)为积分控制项；u_imaxsε和u_iminsε为高低速度积分控制器输出的上限和下限，u_imaxsε＝max(u_maxsε-u_psε(j),0)，u_iminsε＝min(u_minsε-u_psε(j),0)；K_psε为PI比例控制系数；K_isε为积分系数，u_presatsε(j)为高低速度控制量预饱和值；

(18)计算稳定条件下的方位随动伺服驱动器的电流控制指令

u_pcβ(k)＝K_pcβe_cβ(k)

u_presatcβ(k)＝u_pcβ(k)+u_icβ(k)

其中：u_pcβ(j)为比例控制项；u_icβ(j)为积分控制项；u_maxcβ和u_mincβ为方位电流PI控制器输出的上限和下限，u_imaxcβ和u_imincβ为方位电流积分控制器输出的上限和下限，u_imaxcβ(j)＝max(0,u_maxcβ-u_pcβ(k))，u_imincβ(j)＝min(0,u_mincβ-u_pcβ(k))，K_pcβ为PI比例控制系数；K_icβ为积分系数；i_β为方位传动的减速比，u_presatcβ(j)为方位电流控制量预饱和值；

(19)计算稳定条件下的高低随动伺服驱动器的电流控制指令

u_pcε(k)＝K_pcεe_cε(k)

u_presatcε(k)＝u_pcε(k)+u_icε(k)

其中：u_pcε(j)为比例控制项；u_icε(j)为积分控制项；u_maxcε和u_mincε为高低电流PI控制器输出的上限和下限，u_imaxcε和u_imincε为高低电流积分控制器输出的上限和下限，u_imaxcε＝max(u_macxε-u_pcε(k),0，)u_imincε＝min(u_mincε-u_pcε(k),0)；K_pcε为PI比例控制系数；K_icε为积分系数；i_ε为高低随动传动的减速比，e_cε(k)为高低速度等效控制误差，u_presatcε(j)为方位电流控制量预饱和值；

(20)设定驱动器工作在力矩模式，通过CAN总线向伺服驱动器发送电流指令；

(21)若e_β(j)＜e_βmin，e_ε(j)＜e_εmin，通过CAN总线向瞄准手上报瞄准好状态，其中，e_βmin,e_εmin分别为方位和高低控制误差阈值。

本发明的有益效果是：发挥捷联惯导测量火炮身管在大地坐标系下指向的精度高优势，且有效地克服了载体姿态对火炮身管指向控制干扰，以及高低和方位随动的控制耦合问题，实现了高精度的火炮身管指向的稳定、跟踪控制，不易受自行火炮载体运行速度和环境(陆地、海上)的影响，没有额外采用相应的速率陀螺测量身管的调转角速率和干扰的角速率，节约了成本，提高了系统的可靠性。

附图说明

图1是本发明的控制原理图；

图2是本发明的控制传递函数结构图；

图3是本发明的计算流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明实施的步骤是：

(1)启动控制，控制步数为k，设定速度环控制补偿，即控制周期T_s＝1ms，

k＝k+1

取k的初值为0；

(2)采集捷联惯导陀螺组所测量的三轴角速率ω_p(k)＝[ω_px(k) ω_py(k) ω_pz(k)]^T，其中，ω_px(k)为x轴角速率；ω_py(k)为y轴角速率；ω_pz(k)为z轴角速率；

(3)计算身管在大地坐标下的调转角速率

其中：ψ(j),θ(j),

为捷联惯导SINS输出的航向角、俯仰角和横滚角；j为位置环计算步数；

(4)采集方向侧角器和高低侧角器的测量值β_b(k),ε_b(k)；利用非线性观测器提取炮塔在车体上、火炮在炮塔上的调转角速率

其中

e₁(k),e₂(k)为观测误差，α，δ分别为fal函数的参数0.01≤α≤1，0.01≤δ≤1，β₁₁,β₁₂分别为方向测角器观测器的一阶、二阶增益，β₂₁,β₂₂分别为高低测角器观测器的一阶、二阶增益，z₁₁(k)、z₂₁(k)分别为β_b(k),ε_b(k)的估计值，

z₁₂(k)、z₂₂(k)分别为

的估计值。

(5)计算炮塔的横滚角、俯仰和方位轴角速率ω_hx(k),ω_hy(k),ω_hz(k)：

(6)计算车体的航向角速率r(k)：

(7)若k为10的倍数时，执行第(8)步，否则跳转至第(18)步；

j＝j+1

其中j的初值为0，此为位置控制的周期10ms；

(8)接收捷联惯导的航向和姿态角ψ(j),θ(j),

(9)接收大地坐标下的火炮瞄准控制指令ψ_ref(j),θ_ref(j)，其中，ψ_ref(j),θ_ref(j)分别为航向角和俯仰角；

(10)调炮控制误差判断：

e_β(j)＝ψ_ref(j)-ψ(j)

e_ε(j)＝θ_ref(j)-θ(j)

若调炮控制误差e_β(j)≥e_βmax或e_ε(j)≥e_εmax，转入(11)；否则，转入(14)；

(11)计算大幅调转下的方位随动伺服驱动器的速度控制指令

其中：k_eβ为根号e控制系数；u_maxsβ和u_minsβ为方位根号e控制器输出的上限和下限；

(12)计算大幅调转下的高低随动伺服驱动器的速度控制指令

其中：k_eε为根号e控制系数；u_maxsε和u_minsε为高低根号e控制器输出的上限和下限；

(13)设定驱动器工作在调速模式，通过CAN总线分别向方位和高低伺服驱动器发送速度控制指令

(14)计算方位和高低位置控制系统的前馈控制量u_fβ(j),u_fε(j)

其中，k_fβ,k_fε分别为方位和高低系统的前馈控制器系数，

和

分别为航向角速率和俯仰角速率；

(15)计算高低和方位干扰速度补偿量u_dβ(j),u_dε(j)：

其中，

分别为高低和方位随动的干扰速率；k_dβ1,k_dβ2,k_dβ3,分别为方位补偿器的综合系数；k_dε1,k_dε2,k_dε3,分别为高低补偿器的综合系数；

(16)计算稳定条件下的方位随动伺服驱动器的速度控制指令

u_psβ(j)＝K_psβe_sβ(j)

u_presatsβ(j)＝u_psβ(j)+u_isβ(j)+u_fβ(j)+u_dβ(j)

其中：u_psβ(j)为比例控制项；u_isβ(j)为积分控制项；u_imaxsβ和u_iminsβ为方位速度积分控制器输出的上限和下限，u_imaxsβ(j)＝max(0,u_maxsβ-u_psβ(j))，u_iminsβ(j)＝min(0,u_minsβ-u_psβ(j))；K_psβ为PI比例控制系数；K_isβ为积分系数，u_presatsβ(j)为方位速度控制量预饱和值；

(17)计算稳定条件下的高低随动伺服驱动器的速度控制指令

u_psε(j)＝K_psεe_sε(j)

u_presatsε(j)＝u_psε(j)+u_isε(j)+u_fε(j)+u_dε(j)

其中：u_psε(j)为比例控制项；u_isε(j)为积分控制项；u_imaxsε和u_iminsε为高低速度积分控制器输出的上限和下限，u_imaxsε＝max(u_maxsε-u_psε(j),0)，u_iminsε＝min(u_minsε-u_psε(j),0)；K_psε为PI比例控制系数；K_isε为积分系数，u_presatsε(j)为高低速度控制量预饱和值；

(18)计算稳定条件下的方位随动伺服驱动器的电流控制指令

u_pcβ(k)＝K_pcβe_cβ(k)

u_presatcβ(k)＝u_pcβ(k)+u_icβ(k)

其中：u_pcβ(j)为比例控制项；u_icβ(j)为积分控制项；u_maxcβ和u_mincβ为方位电流PI控制器输出的上限和下限，u_imaxcβ和u_imincβ为方位电流积分控制器输出的上限和下限，u_imaxcβ(j)＝max(0,u_maxcβ-u_pcβ(k))，u_imincβ(j)＝min(0,u_mincβ-u_pcβ(k))，K_pcβ为PI比例控制系数；K_icβ为积分系数；i_β为方位传动的减速比，u_presatcβ(j)为方位电流控制量预饱和值；

(19)计算稳定条件下的高低随动伺服驱动器的电流控制指令

u_pcε(k)＝K_pcεe_cε(k)

u_presatcε(k)＝u_pcε(k)+u_icε(k)

其中：u_pcε(j)为比例控制项；u_icε(j)为积分控制项；u_maxcε和u_mincε为高低电流PI控制器输出的上限和下限，u_imaxcε和u_imincε为高低电流积分控制器输出的上限和下限，u_imaxcε＝max(u_macxε-u_pcε(k),0，)u_imincε＝min(u_mincε-u_pcε(k),0)；K_pcε为PI比例控制系数；K_icε为积分系数；i_ε为高低随动传动的减速比，e_cε(k)为高低速度等效控制误差，u_presatcε(j)为方位电流控制量预饱和值；

(20)设定驱动器工作在力矩模式，通过CAN总线向伺服驱动器发送电流指令；

(21)若e_β(j)＜e_βmin，e_ε(j)＜e_εmin，通过CAN总线向瞄准手上报瞄准好状态。

本发明的控制原理见图1。图中利用SINS陀螺组测量火炮身管调转的绝对角速率，它是地球自转角速率、车体姿态变化的角速率、火炮身管相对车体调转角速率的总和。而对控制有效的角速率是火炮身管相对车体调转角速率，其它都可以视为干扰。SINS的陀螺组的角速率可以伺服驱动器构成速度稳定闭环，完成瞄准的速率直接稳定。SINS的航向角和姿态角作为瞄准稳定系统位置环的反馈与位置控制器构成大地坐标系下的位置闭环。通过控制环路的串级控制消除高低和方位两个控制通道的干扰，从而达到理想的稳定瞄准精度。由于该瞄准系统具有火炮身高低和方位侧角器，分别测量身管相对炮塔管俯仰角，炮塔相对车体方位角。因此，根据SINS陀螺组的角速率、高低角和方位侧角，即可推算出影响火炮瞄准稳定的炮塔横滚角速率和俯仰角速率，以及车体航向角速率。从而方便根据实际情况应用设计速率干扰补偿器，来提高火炮瞄准稳定精度。

该控制方法大致步骤：首先，采集捷联惯导陀螺组所测量的三轴角速率ω_p(k)，将其转换至大地坐标系下的角速率

然后，采集侧角器，计算车体系下高低和方位调转角速率

并计算炮塔俯仰和横滚角速率ω_hx(k),ω_hy(k)，以及车体航向角速率r；其次，接受稳定控制指令，采集位置反馈，大幅调转时，控制驱动器工作于调速模式；再再次，进入稳定稳定模式，进行位置、前馈控制，速率稳定、速率干扰补偿控制，控制驱动器工作于电流或力矩模式；最后，当达到稳定精度后，上报瞄准好。

本发明的控制传递函数结构见图2。为了简化传递函数，可以将SINS的航向角和俯仰角测量通过简化为

将耦合干扰视为各种通过的干扰；将高低和方位伺服驱动的电流控制器、逆变器、电流调理、电流力矩系数等构成的闭环简化为一阶惯性环节

方位和高低速度环控制为

K_psβ,K_psε分别为比例系数，K_isβ,K_isε为积分系数；方位和高低位置环控制为

K_pcβ,K_pcε分别为比例系数，K_icβ,K_icε分别为积分系数；方位和高低前馈控制器为k_fβs,k_fεs，k_fβ,k_fε是相应的系数；干扰速率前馈控制器为

f_dβ,f_dε分别为增益系数，T_dβ,T_dε分别为时间常数，采用双线性变换将其离散化。

实施该控制方法的火炮稳定瞄准系统主要由稳定瞄准控制系统、驱动调速系统、动力电源系统。稳定瞄准控制系统采用基于x86的嵌入式计算机。驱动调速系统以DSP28335+CPLD为核心的控制板，驱动功率电子IPM(智能驱动)控制电机转动。方位永磁同步电机(PMSM)，母线电压为540VDC，极对数n_p＝3，额定电流为175A，转矩系数为0.46N.m/A，定子电感为0.4mH，定子电阻为3毫欧，额定转速5500RPM，额定转矩79Nm，电机转子及负载的等效转动惯量J总和为0.03kg·m²；高低PMSM，母线电压为540VDC，极对数n_p＝3，额定电流为38A，转矩系数为0.53N.m/A，定子电感为0.706mH，定子电阻为0.12欧，额定转速为4200RPM，额定转矩38Nm，电机转子及负载的等效转动惯量J总和为0.0068kg·m²。方位负载转动惯量为5800kg·m²，传动速比为210。高低负载转动惯量为900kg·m²。传动速比为275。SINS的角速率测量范围为±300°/s，航向测量精度不大于0.3mil，姿态测量精度不大于0.1mil。

图3为本发明控制方法的计算流程图，下面将结合流程图详述具体实施过程。

(1)启动控制，控制步数为k，设定速度环控制补偿，即控制周期T_s＝1ms，

k＝k+1

取k的初值为0；

(2)采集捷联惯导陀螺组所测量的三轴角速率ω_p(k)＝[ω_px(k) ω_py(k) ω_pz(k)]^T；

(3)计算身管在大地坐标下的调转角速率

其中：ψ(j),θ(j),

为SINS输出的航向角、俯仰角和横滚角；j为位置环计算步数；

(4)采集方向侧角器和高低侧角器的测量值β_b(k),ε_b(k)；利用非线性观测器提取炮塔在车体上、火炮在炮塔上的调转角速率

其中

e₁(k),e₂(k)为观测误差，fal函数的参数α＝0.8，δ＝0.1分别为；观测器的一阶、二阶增益β₁₁＝90,β₁₂＝1350,β₂₁＝90,β₂₂＝1350。

(5)计算炮塔的俯仰和横滚角速率ω_hx(k),ω_hy(k)：

(6)计算车体的航向角速率r(k)：

(7)若k为10的倍数时，执行第(8)步，否则跳转至第(18)步；

j＝j+1

其中j的初值为0，此为位置控制的周期10ms；

(8)接收捷联惯导的航向和姿态角ψ(j),θ(j),

(9)接收大地坐标下的火炮瞄准控制指令ψ_ref(j),θ_ref(j)，包含

(10)调炮控制误差判断：

e_β(j)＝ψ_ref(j)-ψ(j)

e_ε(j)＝θ_ref(j)-θ(j)

若调炮控制误差e_β(j)≥e_βmax或e_ε(j)≥e_εmax，转入步骤(11)；否则，转入步骤(14)；

(11)计算大幅调转下的方位随动伺服驱动器的速度控制指令

其中：根号e控制系数k_eβ＝11.3；控制器输出的上限u_maxsβ＝6000和下限u_minsβ＝-6000；

(12)计算大幅调转下的高低随动伺服驱动器的速度控制指令

其中：根号e控制系数k_eε＝6.5；控制器输出的上限u_maxsε＝4500和下限u_minsε＝-4500；

(13)设定驱动器工作在调速模式，通过CAN总线分别向方位和高低伺服驱动器发送速度控制指令

(14)计算方位和高低位置控制系统的前馈控制量u_fβ(j),u_fε(j)

其中，前馈控制器系数k_fβ＝2.06,k_fε＝2.46，分别与方位调速系统和高调速系统的传递函数关联；

(15)计算高低和方位干扰速度补偿量u_dβ(j),u_dε(j)：

其中，

分别为高低和方位随动的干扰速率；k_dβ1＝1.11,k_dβ2＝-2.3,k_dβ3＝2.3分别为方位补偿器的综合系数；高低补偿器的综合系数k_dε1＝1.11,k_dε2＝-1.56,k_dε3＝1.56；

(16)计算稳定条件下的方位随动伺服驱动器的速度控制指令

u_psβ(j)＝K_psβe_sβ(j)

u_presatsβ(j)＝u_psβ(j)+u_isβ(j)+u_fβ(j)+u_dβ(j)

其中：PI控制器输出的上限u_maxsβ＝6000和下限u_minsβ＝-6000，积分控制器输出的上限u_imaxsβ＝1000和下限u_iminsβ＝-1000；PI比例控制系数K_psβ＝400，积分系数K_isβ＝5；

(17)计算稳定条件下的高低随动伺服驱动器的速度控制指令

u_psε(j)＝K_psεe_sε(j)

u_presatsε(j)＝u_psε(j)+u_isε(j)+u_fε(j)+u_dε(j)

其中：PI控制器输出的上限u_maxsε＝4500和下限u_minsε＝-4500；积分控制器输出的上限u_imaxsε＝1200和下限u_iminsε＝-1200；为PI比例控制系数K_psε＝250，积分系数K_isε＝1；

(18)计算稳定条件下的方位随动伺服驱动器的电流控制指令

u_pcβ(k)＝K_pcβe_cβ(k)

u_presatcβ(k)＝u_pcβ(k)+u_icβ(k)

其中：PI控制器输出的上限u_maxcβ＝525和下限u_mincβ＝-525，积分控制器输出的上限u_imaxcβ＝300和下限u_imincβ＝-300；PI比例控制系数K_pcβ＝30，积分系数K_icβ＝0.1；方位传动的减速比i_β＝210；

(19)计算稳定条件下的高低随动伺服驱动器的电流控制指令

u_pcε(k)＝K_pcεe_cε(k)

u_presatcε(k)＝u_pcε(k)+u_icε(k)

其中：PI控制器输出的上限u_maxcε＝114和下限u_mincε＝-114，积分控制器输出的上限u_imaxcε＝50和下限u_imincε＝-50；PI比例控制系数K_pcε＝20；积分系数K_icε＝1；高低随动传动的减速比i_ε＝275；

(20)设定驱动器工作在力矩模式，通过CAN总线向伺服驱动器发送电流指令；

(21)若e_β(j)＜e_βmin,e_ε(j)＜e_εmin，通过CAN总线向瞄准手上报瞄准好状态。

所用参数的范围取值范围见下表：

Claims (1)

1.一种采用捷联惯导的火炮瞄准稳定系统控制方法，其特征在于包括下述步骤：

(1)设定速度环控制补偿的控制步数k的初值为0，k逐一增加；设定速度环控制补偿的控制周期T_s＝1ms；

(2)采集捷联惯导陀螺组所测量的三轴角速率ω_p(k)＝[ω_px(k) ω_py(k) ω_pz(k)]^T，其中，ω_px(k)为x轴角速率；ω_py(k)为y轴角速率；ω_pz(k)为z轴角速率；

(3)计算身管在大地坐标下的航向、俯仰和横滚调转角速率

其中，ψ(j),θ(j),

为捷联惯导SINS输出的航向角、俯仰角和横滚角；j为位置环计算步数；

(4)采集方向侧角器和高低侧角器的测量值β_b(k),ε_b(k)；利用非线性观测器提取炮塔在车体上、火炮在炮塔上的调转角速率

其中

e₁(k),e₂(k)为观测误差，α，δ分别为fal函数的参数0.01≤α≤1，0.01≤δ≤1，β₁₁,β₁₂分别为方向测角器观测器的一阶、二阶增益，β₂₁,β₂₂分别为高低测角器观测器的一阶、二阶增益，z₁₁(k)、z₂₁(k)分别为β_b(k),ε_b(k)的估计值，z₁₂(k)、z₂₂(k)分别为

的估计值；

(5)计算炮塔的横滚角、俯仰和方位轴角速率ω_hx(k),ω_hy(k),ω_hz(k)：

(6)计算车体的航向角速率

(7)设定位置控制的位置环计算步数j的初值为0，j逐一增加，位置控制的控制周期为10ms；判定k值，若k为10的倍数时，执行第(8)步，否则跳转至第(18)步；

(8)接收捷联惯导的航向、姿态和横滚角ψ(j),θ(j),

(9)接收大地坐标下的火炮瞄准控制指令ψ_ref(j),θ_ref(j)，其中，ψ_ref(j),θ_ref(j)分别为航向角和俯仰角；

(10)判断方位调炮控制误差e_β(j)＝ψ_ref(j)-ψ(j)，高低调炮控制误差e_ε(j)＝θ_ref(j)-θ(j)，若调炮控制误差e_β(j)≥e_βmax或e_ε(j)≥e_εmax，转入步骤(11)；否则，转入步骤(14)，其中，e_βmax,e_εmax分别为方位和高低分段控制误差阈值；

(11)计算大幅调转下的方位随动伺服驱动器的速度控制指令

其中：k_eβ为根号e控制系数；u_maxsβ和u_minsβ为方位根号e控制器输出的上限和下限；

(12)计算大幅调转下的高低随动伺服驱动器的速度控制指令

其中：k_eε为根号e控制系数；u_maxsε和u_minsε为高低根号e控制器输出的上限和下限；

(13)设定驱动器工作在调速模式，通过CAN总线分别向方位和高低伺服驱动器发送速度控制指令

(14)计算方位和高低位置控制系统的前馈控制量u_fβ(j),u_fε(j)

其中，k_fβ,k_fε分别为方位和高低系统的前馈控制器系数，

和

分别为航向角速率和俯仰角速率；

(15)计算高低和方位干扰速度补偿量u_dβ(j),u_dε(j)：

其中，

分别为高低和方位随动的干扰速率；k_dβ1,k_dβ2,k_dβ3分别为方位补偿器的综合系数；k_dε1,k_dε2,k_dε3,分别为高低补偿器的综合系数；

(16)计算稳定条件下的方位随动伺服驱动器的速度控制指令

并令方位角度等效误差为e_sβ(j)＝e_β(j)；

u_psβ(j)＝K_psβe_sβ(j)

u_presatsβ(j)＝u_psβ(j)+u_isβ(j)+u_fβ(j)+u_dβ(j)

其中：u_psβ(j)为比例控制项；u_isβ(j)为积分控制项；u_imaxsβ和u_iminsβ为方位速度积分控制器输出的上限和下限，u_imaxsβ(j)＝max(0,u_maxsβ-u_psβ(j))，u_iminsβ(j)＝min(0,u_minsβ-u_psβ(j))；K_psβ为PI比例控制系数；K_isβ为积分系数，u_presatsβ(j)为方位速度控制量预饱和值；

(17)计算稳定条件下的高低随动伺服驱动器的速度控制指令

并令高低角度等效误差e_sε(j)＝e_ε(j)；

u_psε(j)＝K_psεe_sε(j)

u_presatsε(j)＝u_psε(j)+u_isε(j)+u_fε(j)+u_dε(j)

其中：u_psε(j)为比例控制项；u_isε(j)为积分控制项；u_imaxsε和u_iminsε为高低速度积分控制器输出的上限和下限，u_imaxsε＝max(u_maxsε-u_psε(j),0)，u_iminsε＝min(u_minsε-u_psε(j),0)；K_psε为PI比例控制系数；K_isε为积分系数，u_presatsε(j)为高低速度控制量预饱和值；

(18)计算稳定条件下的方位随动伺服驱动器的电流控制指令

u_pcβ(k)＝K_pcβe_cβ(k)

u_presatcβ(k)＝u_pcβ(k)+u_icβ(k)

其中：u_pcβ(j)为比例控制项；u_icβ(j)为积分控制项；u_maxcβ和u_mincβ为方位电流PI控制器输出的上限和下限，u_imaxcβ和u_imincβ为方位电流积分控制器输出的上限和下限，u_imaxcβ(j)＝max(0,u_maxcβ-u_pcβ(k))，u_imincβ(j)＝min(0,u_mincβ-u_pcβ(k))，K_pcβ为PI比例控制系数；K_icβ为积分系数；i_β为方位传动的减速比，u_presatcβ(j)为方位电流控制量预饱和值；

(19)计算稳定条件下的高低随动伺服驱动器的电流控制指令

u_pcε(k)＝K_pcεe_cε(k)

u_presatcε(k)＝u_pcε(k)+u_icε(k)

其中：u_pcε(j)为比例控制项；u_icε(j)为积分控制项；u_maxcε和u_mincε为高低电流PI控制器输出的上限和下限，u_imaxcε和u_imincε为高低电流积分控制器输出的上限和下限，u_imaxcε＝max(u_maxcε-u_pcε(k),0)，u_imincε＝min(u_mincε-u_pcε(k),0)；K_pcε为PI比例控制系数；K_icε为积分系数；i_ε为高低随动传动的减速比，e_cε(k)为高低速度等效控制误差，u_presatcε(j)为方位电流控制量预饱和值；

(20)设定驱动器工作在力矩模式，通过CAN总线向伺服驱动器发送电流指令；

(21)若e_β(j)＜e_βmin，e_ε(j)＜e_εmin，通过CAN总线向瞄准手上报瞄准好状态，其中，e_βmin,e_εmin分别为方位和高低控制误差阈值。

Images