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

权利要求书

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

(1)设定控制步数i初值为0；设定控制或采样周期，每个控制或采样周期步数i加1；

(2)采集车体上SINS的航姿角ψ_b(i),θ_b(i),其中ψ_b(i)、θ_b(i)、分别为SINS的航向角、俯仰角、横滚角；

(3)采集高低受信仪的测量值ε_b(i)和方位受信仪的测量值β_b(i)；

(4)采集高低电机轴角度值ε_m(i)及其角速度ω_ε(i)、方位电机轴角度值β_m(i)及其角速度ω_β(i)；

(5)采集SINS的陀螺组测量的三轴角速率ω_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T；

(6)采集炮塔陀螺组测量的三轴角速率ω_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T；

(7)采集火炮俯仰陀螺测量的角速率ω_p(j)；

(8)通过高低和方向受信仪等效闭环角度计算火炮身管在空间中的指向；

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

(10)计算方位位置控制策略，包括方位位置控制误差、方位速度指令、方位滑模等效控制量、方位滑模变结构控制量、方位滑模控切换制量；

(11)计算高低位置控制策略，包括高低位置控制误差、高低速度指令、高低滑模等效控制量、高低滑模变结构控制量、高低滑模控切换制量；

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

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

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

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

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

(17)设定驱动器工作在力矩模式，通过CAN总线向伺服驱动器发送电流指令，转入第(1)步。

2.根据权利要求1所述的大地坐标下等效闭环干扰速率补偿自稳定控制方法，其特征在于，所述的步骤(8)通过高低和方向受信仪等效闭环角度计算火炮身管在空间中的指向，

其中，ψ(i)为火炮身管的航向角，θ(i)为火炮身管的俯仰角，为火炮身管的横滚角，ψ′、为计算过程中的中间变量，为火炮身管姿态矩阵，

其中，为车体姿态矩阵，为车体航向姿态矩阵，为车体俯仰姿态矩阵，为车体横滚姿态矩阵，为火炮方位姿态矩阵，为火炮俯仰姿态矩阵。