具体实施方式

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

本发明针对控制系统对齿隙和弹性比较敏感，将SINS安装在车体底盘上，炮塔陀螺组仍然安装在炮塔上，将原来闭环稳定的车体陀螺安装在火炮摇架上，随火炮俯仰。高低和方向受信仪采用等效闭环的方式间接测量火炮在车体坐标系的高低和方位角(也称半闭环)进行控制，并进行推算火炮身管在大地坐标系下的指向，使控制系统将传动机构的较大齿隙和弹性结构谐振部分排除在控制闭环外。同时该控制方法采用反步高阶滑模齿隙补偿控制方法，从而使得控制系统的稳定性更强，进一步提高稳定控制精度。本发明实施的步骤是：

(1)启动控制，控制步数i，即控制或采样周期T_s＝1ms，步数i加1，i的初值为0；

(2)采集安装在车体上SINS的航姿角ψ_b(i),θ_b(i),其中ψ_b(i)为惯导的航向角；θ_b(i)为惯导的俯仰角；为惯导的横滚角；

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

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

(5)采集SINS的陀螺组所测量的三轴角速率ω_b(j)，其中ω_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T，ω_b1(j),ω_b2(j),ω_b3(j)分别惯导三轴陀螺轴1、轴2、轴3角速率陀螺的测量值；

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

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

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

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

其中：为车体姿态矩阵；为火炮身管姿态矩阵；为车体航向姿态矩阵，为车体俯仰姿态矩阵，为车体横滚姿态矩阵；

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

(10)方位位置控制策略计算

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

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

其中：e_β(i)为方位位置控制误差；为方位速度指令；u_pβmax,u_pβmin为方位转速上下限幅值；J_1β为方位炮塔转动惯量；η_β为方位传动速比；k_β方位传动刚度；u_eqβ(i)为方位滑模等效控制量；u_swβ(i)方位滑模变结构控制量；b_β为方位摩擦系数；α_β为方位传动齿隙；ε(Δβ_m)为方位齿隙拟合非线性函数；Δβ_m为方位电机轴与方位测角值之差；β_m(i)为方位电机轴角度；c_1β,c_2β分别为方位滑模一、二阶系数，α_1β,α_2β分别为其滑模一、二阶指数；T_s为采样周期；τ为拟合系数；s_1β为方位控制定义的滑模面；v_β(i)为方位滑模控切换制量；z_0β(j),z_1β(j),z_2β(j)分别是方位位置控制误差e_β(i)的0阶、一阶、二阶估计值，λ_0β,λ_1β,λ_2β分别为其状态估计的0阶、一阶、二阶系数，v_0β,v_1β分别为其状态估计中的中间变量；T_β为方位滤波时间常数；k_dβ为高低滑模稳定系数，κ_1β为高低滑模收敛系数。

(11)高低位置控制策略计算

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

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

其中：e_ε(i)为高低位置控制误差；为高低速度指令；u_pεmax,u_pεmin为高低转速上下限幅值；J_1ε为高低炮塔转动惯量；η_ε为高低传动速比；k_ε高低传动刚度；u_eqε(i)为高低滑模等效控制量；u_swε(i)高低滑模变结构控制量；b_ε为高低摩擦系数；α_ε为高低传动齿隙；ε(Δε_m)为高低齿隙拟合非线性函数；ε_m(i)为高低电机轴角度；Δε_m为高低电机轴与高低测角的角度差；c_1ε,c_2ε分别为高低滑模一、二阶系数，α_1ε,α_2ε分别为其滑模一、二阶指数；s_1ε为高低控制定义的滑模面；v_ε(i)为高低滑模控切换制量；

z_0ε(j),z_1ε(j),z_2ε(j)分别是高低位置控制误差e_ε(i)的0阶、一阶、二阶估计值，λ_0ε,λ_1ε,λ_2ε分别为其状态估计的0阶、一阶、二阶系数，v_0ε,v_1ε分别为其状态估计中的中间变量；T_ε为高低滤波时间常数；k_dε为高低滑模稳定系数，κ_1ε为高低滑模收敛系数。。

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

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

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

其中，c₁₁,c₁₂,d₁₁为高低干扰滤波校正系数；c₂₁,c₂₂,d₂₁为方位干扰滤波校正系数；

其中：T_s为速度控制周期；T_ε1，T_β1分别为高低和方位滤波时间系数；T_ε2，T_β2分别为高低和方位时间特性常数；k_ε1，k_β1分别为高低和方位增益常数。

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

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

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

其中：i_qeqβ,i_qnβ分别为方位终端滑模等效控制量和滑模积分控制量；i_qβmax,i_qβmin分别为方位电流限幅值；J_β为方位电机负载转动惯量；p_β为方位电机极对数；ψ_fβ为方位电机磁链系数；B_β方位系统综合粘性摩擦系数；γ_β,q_β,P_β方位终端滑模系数；k_β,η_β1,η_β2为方位终端滑模控制系数；z_0lβ(j)，z_1lβ(j)分别是终端滑模l_eβ(j)的估计值和一阶估计值；λ_0lβ，λ_1lβ分别为l_eβ(j)估计的0阶和一阶估计系数；z_0ωβ(j)，z_1ωβ(j)分别是方位速度指令的估计值和一阶估计值；λ_0ωβ，λ_1ωβ分别为估计的0阶和一阶估计系数；z_0eβ(j)，z_1eβ(j)分别是方位速度控制误差e_βω(j)的估计值和一阶估计值；λ_0eβ，λ_1eβ分别为e_βω(j)估计的0阶和一阶估计系数；v_0lβ,v_0ωβ,v_0eβ分别为方位状态估计的中间变量。

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

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

其中：i_qeqε,i_qnε分别为高低终端滑模等效控制量和滑模积分控制量；i_qεmax,i_qεmin分别为高低电流限幅值；J_ε为高低电机负载转动惯量；p_ε为方高低电机极对数；ψ_fε为高低电机磁链系数；B_ε高低系统综合粘性摩擦系数；γ_ε,q_ε,P_ε高低终端滑模系数；k_ε,η_ε1,η_ε2为高低终端滑模控制系数；z_0lε(j)，z_1lε(j)分别是高低终端滑模l_eε(j)的估计值和一阶估计值；λ_0lε，λ_1lε分别为l_eε(j)估计的0阶和一阶估计系数；z_0ωε(j)，z_1ωε(j)分别是高低速度指令的估计值和一阶估计值；λ_0ωε，λ_1ωε分别为估计的0阶和一阶估计系数；z_0eε(j)，z_1eε(j)分别是高低速度控制误差e_εω(j)的估计值和一阶估计值；λ_0eε，λ_1eε分别为e_εω(j)估计的0阶和一阶估计系数；v_0lε,v_0ωε,v_0eε分别为高低状态估计的中间变量。

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

本发明实施例的控制原理见图1。图中，高低和方向受信仪是通过与动力传动数比一样的仪表链与电机轴相连，仪表链的传动刚度较大和齿隙较小。火炮身管在大地坐标系的指向通过安装在车体底盘上的捷联惯导的航姿和高低和受信仪测量值推算得到，将该火炮身管指向值作为瞄准位置控制环的反馈，可以实现火炮身管的瞄准。同时，将动力传动的齿隙等非线性排除在位置闭环控制外。稳定系统可根据SINS陀螺组的角速率、高低角、炮塔陀螺组、火炮俯仰陀螺，即可计算出火炮在大地坐标下的方位和高低干扰角速率。稳定系统通过位置控制外环的串级控制消除高低和方位两个控制通道的干扰，从而达到理想的稳定瞄准精度。

该控制方法大致步骤：首先，采集SINS的航向角、俯仰角和横滚角，以及火炮的高低角和方位角、高低和方位电机轴角度与角速度；然后，采集SINS陀螺组、炮塔陀螺组、火炮俯仰陀螺的角速度值；其次，计算火炮身管在大地坐标系下的指向；再次，进行方位和高低控制策略计算；再次，进行干扰角速率的滤波校正，得到方位和高低补偿角速率控制量；再再次，合并稳定控制的速率控制指令和干扰补偿角速率控制指令，作为伺服驱动的速度总指令；最后，根据速度控制指令进行高阶滑模速度控制策略计算得到电流环控制指令，以驱动电机按给定的控制量控制火炮调转运动。

本发明实施例的控制传递函数结构见图2。为了简化传递函数，可以将SINS的航向角和俯仰角测量通过简化为将耦合干扰视为各种通过的干扰；将高低和方位伺服驱动的电流控制器、逆变器、电流调理、电流力矩系数等构成的闭环简化为一阶惯性环节方位和高低速度环控制为高阶滑模控制；方位和高低位置环控制也为高阶滑模控制；干扰速率前馈控制器为f_dβ,f_dε分别为增益系数，T_dβ,T_dε分别为时间常数，采用双线性变换将其离散化。

实施该控制方法的火炮稳定瞄准系统主要由稳定控制系统、驱动调速系统、动力电源系统、炮塔陀螺组、车体陀螺、高低和方位侧角器等组成。稳定瞄准控制系统采用基于x86的嵌入式计算机。驱动调速系统以DSP28335+FPGA为核心的控制板，驱动功率电子IPM(智能驱动)控制电机转动。方位永磁同步电机(PMSM)，母线电压为56VDC，极对数n_p＝3，额定功率4kW，定子电感为0.0098mH，定子电阻为3.5毫欧，额定转速3000RPM，额定转矩7.4Nm，电机转子及传动齿轮系的的等效转动惯量J总和为0.013kg·m²；高低PMSM，母线电压为56VDC，极对数n_p＝3，额定功率2kW，额定转矩3.2Nm，定子电感为0.032mH，定子电阻为0.0105欧，额定转速为3000RPM，电机转子及传动齿轮系的等效转动惯量J总和为0.0075kg·m²。方位负载转动惯量约为2700kg·m²，传动速比为470。高低负载转动惯量为700kg·m²。传动速比为450。SINS的角速率测量范围为±300°/s，航向测量精度不大于0.3mil，姿态测量精度不大于0.1mil。

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

(1)启动控制，控制步数i，即控制或采样周期T_s＝1ms

i＝i+1

其中i的初值为0；

(2)采集安装在车体上SINS的航姿角ψ_b(i),θ_b(i),其中ψ_b(i)为惯导的航向角；θ_b(i)为惯导的俯仰角；为惯导的横滚角；

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

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

(5)采集SINS的陀螺组所测量的三轴角速率ω_b(j)，其中ω_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T，ω_b1(j),ω_b2(j),ω_b3(j)分别惯导三轴陀螺轴1、轴2、轴3角速率陀螺的测量值；

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

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

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

将幅度转换为密位；

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

(10)方位位置控制策略计算

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

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

其中：给定方位电机额定转速u_pβmax＝3000，u_pβmin＝-3000；炮塔转动惯量J_1β＝2700；给定的速比η_β＝470；刚度k_β＝2×10⁷；摩擦系数b_β＝0.005；取测量值齿隙α_β＝1.2。以下设计参数：c_1β＝15，c_2β＝9，τ＝27，λ_0β＝5，λ_1β＝21，λ_2β＝250，T_β＝0.001，k_dβ＝15，κ_1β＝150。

(11)高低位置控制策略计算

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

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

其中：给定高低电机额定转速u_pεmax＝3000，u_pεmin＝-3000；火炮俯仰惯量J_1ε＝700；给定的速比η_ε＝450；刚度k_ε＝1.8×10⁷；摩擦系数b_ε＝0.008；取测量值齿隙α_ε＝0.9。以下设计参数：c_1ε＝23，c_2ε＝5，τ＝27，λ_0ε＝8.15，λ_1ε＝36.7，λ_2ε＝430，T_ε＝0.0013，k_dε＝17.1，，κ_1ε＝203。

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

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

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

其中，c₁₁,c₁₂,d₁₁为高低干扰滤波校正系数；c₂₁,c₂₂,d₂₁为方位干扰滤波校正系数；

其中：T_s＝0.001；T_ε1＝0.01，T_β1＝0.01；T_ε2＝0.025，T_β2＝0.036；k_ε1＝4.5，k_β1＝4.7；

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

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

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

其中：给定方位电机限流值i_qβmax＝150,i_qβmin＝-150；方位电机轴J_β＝0.013，其电机极对数p_β＝3，其磁链系数ψ_fβ＝0.0031，摩擦系数B_β＝0.005。设计参数如下：γ_β＝0.02，q_β＝3，P_β＝5；k_β＝250，η_β1＝300，η_β2＝2000；λ_0lβ＝5.321，λ_1lβ＝15.487；λ_0ωβ＝2.56，λ_1ωβ＝11.654；λ_0eβ＝7.662，λ_1eβ＝18.956。

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

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

其中：给定高低电机限流值i_qεmax＝75,i_qεmin＝-75；高低电机轴J_ε＝0.0075，其极对数p_ε＝3，其磁链系数ψ_fε＝0.02，给定摩擦系数B_ε＝0.008。设计参数如下：γ_ε＝0.01，q_ε＝5，P_ε＝7；k_ε＝300，η_ε1＝450，η_ε2＝1500；λ_0lε＝4.365，λ_1lε＝13.78；λ_0ωε＝5.862，λ_1ωε＝14.953；λ_0eε＝7.632，λ_1eε＝19.564。

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

本实施例所用参数的范围见表1。

表1参数取值范围