[发明专利]采用捷联惯导的火炮瞄准稳定系统控制方法

权利要求书

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分别为方位和高低控制误差阈值。