[发明专利]一种大地坐标系下全闭环干扰速率补偿自稳定控制方法

权利要求书

1.一种大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，包括以下步骤：

(1)启动控制，设定控制步数为j和i，其中j为稳定系统速度环控制步数，控制周期T_s＝1ms；i为稳定系统位置环的控制步数，控制周期T＝10ms；j和i初值为0；

(2)判断j％10＝0是否成立，若是，控制步数i值加1，并转入第(3)步；否则转入第(9)步；

(3)采集SINS的航姿角ψ(i),θ(i),其中ψ(i)为SINS的航向角；θ(i)为SINS的俯仰角；为SINS的横滚角；

(4)计算航向角和俯仰角的带阻滤波ψ′(i)和θ′(i)；

(5)采集高低侧角器测量值ε_b(i)和方位侧角器的测量值β_b(i)；

(6)判断是否同时接收到大地坐标下的火炮瞄准方位控制指令ψ_ref(j)、高低控制指令θ_ref(j)，若是，进入第(7)步；否则转入第(9)步；

(7)求取方位位置控制误差e_β(i)和高低位置控制误差e_ε(i)；

(8)若方位位置控制误差大于设定阈值，计算大幅调转下的方位随动伺服驱动器的速度控制指令

若高低位置控制误差大于设定阈值，计算大幅调转下的高低随动伺服驱动器的速度控制指令

若方位位置控制误差小于等于设定阈值，计算稳定条件下的方位随动伺服驱动器的速度控制指令

若高低位置控制误差小于等于设定阈值，计算稳定条件下的高低随动伺服驱动器的速度控制指令

(9)采集安装在火炮摇架上SINS的陀螺组所测量的三轴角速率ω_p(j)，其中ω_p(j)＝[ω_p1(j),ω_p2(j),ω_p3(j)]^T，ω_p1(j),ω_p2(j),ω_p3(j)分别为惯导陀螺组的三轴角速率的测量值；

(10)采集炮塔陀螺组三轴陀螺所测量的三轴角速率ω_h(j)，其中ω_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T，ω_h1(j),ω_h2(j),ω_h3(j)分别为炮塔陀螺组的三轴角速率的测量值；

(11)采集车体陀螺组测量的角速率ω_b(j)；

(12)计算高低补偿角速率d_ε(j)和方位干扰补偿角速率d_β(j)；

(13)计算高低干扰滤校正量u_dε(j)和方位干扰滤波校正量u_dβ(j)；

(14)计算高低伺服总的速度指令和方位伺服驱动总的速度指令

(15)计算方位驱动的电流指令

(16)计算高低驱动的电流指令

(17)将方位驱动的电流指令和高低驱动的电流指令分别发送给高低和方位电流环。