CN112696981B - 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 - Google Patents

Abstract

本发明提供了一种大地坐标系下全闭环干扰速率补偿自稳定控制方法，采集SINS的航向角、俯仰角和横滚角，以及火炮的高低角和方位角；采集SINS陀螺组、炮塔陀螺组、车体陀螺的角速度值，提取大地坐标系下方位向和高低向的干扰角速率，进行滤波校正，得到方位和高低补偿角速率控制量；合并稳定控制的速率控制指令和干扰补偿角速率控制指令，作为伺服驱动的速度总指令；高低和方位伺服驱动器采集各自电机角速率，并根据速度控制指令进行高阶滑模速度控制策略计算得到电流环控制指令，驱动电机按给定的控制量控制火炮调转运动。本发明具有响应带宽高、干扰速率补偿精准等优点，实现了动基座大射角条件下的火炮身管指向高精度稳定。

Description

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

技术领域

本发明属于火炮稳定控制系统领域，主要涉及需要高动态动基座下精确稳定的火炮随动系统控制方法。

背景技术

随着军事斗争的发展演变，新军事作战需求迫切需要压制武器具备行进间射击的能力，需要自行火炮能够遂行动对静、动对动的作战任务。那么火炮需要在瞄准稳定系统的控制下，克服车体由于行走的路面对车体造成的扰动，保持火炮指向稳定。此类功能在坦克武器或两栖突击炮的炮控系统早已得到实现。

专利“采用捷联惯导的火炮瞄准稳定系统控制方法”应用于力矩电机直接拖动或者性能优异减速机的自行火炮稳定系统上，其主要特点是采用捷联惯导(SINS)的航姿作为火炮随动系统的空间角度反馈，和采用SINS中的三轴角速率陀螺作为速度闭环及干扰速率补偿的速率敏感器件，并观测高低侧角器和方位侧角器的角速度，获得炮塔和车体炮塔的角速率，从而完成火炮的稳定控制。但在工程中发现，传统压制火炮经过一定的适应改进，实现火炮大射角下火炮自稳定，减速机性能较差，欠刚度，齿隙大，难以满足陀螺速度闭环的要求，甚至影响测量火炮身管指向的捷联惯导作为位置全闭环控制系统的稳定性，造成小误差抖动，齿隙“冲振”现象明显。通过安装在控制末端的捷联惯导中的陀螺推算炮塔速率和车体速率，由于受传动齿隙和刚度影响，会携带谐振产生的附加干扰角速度，其推算结果也难以满足使用要求。因而，该方法在传统高速电机结合低性能减速机上火炮难以实现火炮指向在高动态环境下精确稳定瞄准。

发明内容

为了克服现有技术的不足，本发明提供一种大地坐标系下全闭环干扰速率补偿自稳定控制方法，采用SINS作为火炮随动系统的空间角度全闭环控制系统，抑制车体姿态对火炮身管指向的扰动，实现火炮的高精度自稳定控制方法问题，同时实现高低向和方位向随动的控制解耦。针对利用SINS直接测量火炮身管指向，提高指向控制精度，又可以使自行火炮武器系统完成自主定位导航功能，本发明仍然保留传统火炮随动系统组成的侧角器，采用SINS的陀螺组、高低侧角器，炮塔陀螺组、车体陀螺等提取火炮干扰量，实现火炮的全闭环高精度自稳定。

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

(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)将高低电流指令

和方位电流指令

分别发送给高低和方位电流环。

所述的航向角和俯仰角的带阻滤波：

ψ′(i)＝b₁₀ψ(i)+b₁₁ψ(i-1)+b₁₂ψ(i-2)-a₁₁ψ′(i-1)-a₁₂ψ′(i-1)

θ′(i)＝b₂₀θ(i)+b₂₁θ(i-1)+b₂₂θ(i-2)-a₂₁θ′(i-1)-a₂₂θ′(i-1)

其中：b₁₀，b₁₁，b₁₂，a₁₁，a₁₂,为航向角滤波系数；b₂₀，b₂₁，b₂₂，a₂₁，a₂₂为俯仰角滤波系数；ψ′(i)为航向角ψ(i)滤波值；θ′(i)为俯仰角θ(i)滤波值；

其中T为位置采样周期；ω_n为谐振中心频率；k₁为陷波器深度系数，k₂为陷波器宽度系数。

所述的方位位置控制误差e_β(i)＝ψ_ref(i)-ψ′(i)，高低位置控制误差e_ε(i)＝θ_ref(i)-θ′(i)。

所述的方位位置和高低位置控制误差的设定阈值小于3mil/mrad。

所述的大幅调转下的方位随动伺服驱动器的速度控制指令

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

所述的大幅调转下的高低随动伺服驱动器的速度控制指令

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

所述的稳定条件下的方位随动伺服驱动器的速度控制指令

其中，

为方位位置PI控制量，K_psβ和K_isβ为其PI比例控制系数和积分系数；

所述的稳定条件下的高低随动伺服驱动器的速度控制指令；

其中：

为高低位置PI控制量，K_psε和K_isε为其PI比例控制系数和积分系数。

所述的高低补偿角速率

方位干扰补偿角速率

所述的高低干扰滤校正量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分别为高低和方位增益常数；

所述的高低伺服总的速度指令

方位伺服驱动总的速度指令

所述的方位驱动的电流指令

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_β方位系统综合粘性摩擦系数；ω_β(j)为方位电机转速；γ_β,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β分别为状态估计的中间变量。

所述的高低驱动的电流指令

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ε(j)＝e_εω(j)+γ_ε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_ε高低系统综合粘性摩擦系数；ω_ε(j)为高低电机转速；γ_ε,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ε分别为状态估计的中间变量。

本发明的有益效果是：使稳定系统对干扰的抑制控制完全由敏感的干扰速率决定，具有响应带宽高、干扰速率补偿精准等优点，且有效地克服了载体姿态对火炮身管指向控制干扰，高低和方位的指向控制独立，不耦合，充分发挥了SINS测量火炮身管在大地坐标系下指向实现全闭环控制精度高的优势，同时也抑制了传动机构因刚度不足导致的位置外环谐振，提高了弹性负载力矩干扰导致的伺服驱动速度控制精度，使速度控制与系统参数无关，只与终端滑模参数有关，极大提升了干扰速度补偿精度，系统鲁棒性和稳定性大大增强，实现了动基座大射角条件下的火炮身管指向高精度稳定。

附图说明

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

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

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

具体实施方式

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

本发明包括以下步骤：

(1)启动控制，设定控制步数为j,i，且：

j＝j+1

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

(2)判断j％10＝0？是，i＝i+1，并转入第(3)步；否则转入(9)步；

(3)采集SINS的航姿角ψ(i),θ(i),

其中其中ψ(i)为SINS的航向角；θ(i)为SINS的俯仰角；

为SINS的横滚角；

(4)航向角和俯仰角的带阻滤波：

ψ′(i)＝b₁₀ψ(i)+b₁₁ψ(i-1)+b₁₂ψ(i-2)-a₁₁ψ′(i-1)-a₁₂ψ′(i-1)

θ′(i)＝b₂₀θ(i)+b₂₁θ(i-1)+b₂₂θ(i-2)-a₂₁θ′(i-1)-a₂₂θ′(i-1)

其中：b₁₀，b₁₁，b₁₂，a₁₁，a₁₂,为航向角滤波系数；b₂₀，b₂₁，b₂₂，a₂₁，a₂₂为俯仰角滤波系数；ψ′(i)为航向角ψ(i)滤波值；θ′(i)为俯仰角θ(i)滤波值；

其中T为位置采样周期；ω_n为谐振中心频率；k₁为陷波器深度系数，k₂为陷波器宽度系数；

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

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

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

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

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

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

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

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

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

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

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

其中：u_sβ为方位位置PI控制量，K_psβ、K_isβ为其PI比例控制系数和积分系数；

其中：u_ss为高低位置PI控制量，K_psε、K_isε为其PI比例控制系数和积分系数；

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

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

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

(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_β方位系统综合粘性摩擦系数；ω_β(j)为方位电机转速；γ_β,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)

v_0ωε＝z_1ωε(j-1)-λ_0ωε|z_0ωε(j-1)-ω_ε ^*(j-1)|^0.5sgn(z_0ωε(j-1)-ω_ε ^*(j-1))

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

l_eε(j)＝e_εω(j)+γ_ε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_ε高低系统综合粘性摩擦系数；ω_ε(j)为高低电机转速；γ_ε,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)将高低电流指令

和方位电流指令

分别发送给高低和方位电流环。

本发明实施例的控制原理见图1。图中利用SINS陀螺组测量火炮身管调转的绝对角速率，包括地球自转角速率、车体姿态变化的角速率、火炮身管相对车体调转角速率。而对控制有效的角速率是火炮身管相对车体调转角速率，其它都可以视为干扰。SINS的航向角和姿态角作为瞄准稳定系统位置环的反馈与位置控制器构成大地坐标系下的位置闭环。由于该稳定系统具有火炮身高低和方位侧角器，分别测量身管相对炮塔管俯仰角，炮塔相对车体方位角。稳定系统可根据SINS陀螺组的角速率、高低角、炮塔陀螺组、车体陀螺，即可计算出火炮在大地坐标下的方位和高低干扰角速率。稳定系统通过位置控制外环的串级控制消除高低和方位两个控制通道的干扰，从而达到理想的稳定瞄准精度。

本发明实施例的大致步骤包括：首先，采集SINS的航向角、俯仰角和横滚角，以及采集火炮的高低角和方位角，并对航向角和俯仰角进行带阻滤波；然后，接受稳定控制指令，进行位置闭环控制，得到驱动器的速度指令；其次，采集SINS陀螺组、炮塔陀螺组、车体陀螺的角速度值，提取大地坐标系下的方位向和高低向的干扰角速率；再次，进行干扰角速率的滤波校正，得到方位和高低补偿角速率控制量；再再次，合并稳定控制的速率控制指令和干扰补偿角速率控制指令，作为伺服驱动的速度总指令；最后，高低和方位伺服驱动器采集各自电机角速率，并根据速度控制指令进行高阶滑模速度控制策略计算得到电流环控制指令，以驱动电机按给定的控制量控制火炮调转运动。

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

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

方位和高低速度环控制为

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

K_pcβ,K_pcε分别为比例系数，K_icβ,K_icε分别为积分系数；干扰速率前馈控制器为

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)启动控制，设定控制步数为j,i，且：

j＝j+1

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

(2)判断j％10＝0？是，i＝i+1，并转入第(3)步；否则转入(9)步；

(3)采集SINS的航姿角ψ(i),θ(i),

其中其中ψ(i)为SINS的航向角；θ(i)为SINS的俯仰角；

为SINS的横滚角；

(4)航向角和俯仰角的带阻滤波：

ψ′(i)＝b₁₀ψ(i)+b₁₁ψ(i-1)+b₁₂ψ(i-2)-a₁₁ψ′(i-1)-a₁₂ψ′(i-1)

θ′(i)＝b₂₀θ(i)+b₂₁θ(i-1)+b₂₂θ(i-2)-a₂₁θ′(i-1)-a₂₂θ′(i-1)

其中：b₁₀，b₁₁，b₁₂，a₁₁，a₁₂,为航向角滤波系数；b₂₀，b₂₁，b₂₂，a₂₁，a₂₂为俯仰角滤波系数；ψ′(i)为航向角ψ(i)滤波值；θ′(i)为俯仰角θ(i)滤波值；

其中T＝0.01s；ω_n＝3.5×2π；k₁＝100；k₂＝8；

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

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

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

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

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

(8)若e_β＞10mil，计算大幅调转下的方位随动伺服驱动器的速度控制指令

其中：k_eβ＝120；u_maxsβ＝3000，u_minsβ＝-3000，此二者参数有方位电机额定转速决定；

若e_ε＞10mil，计算大幅调转下的高低随动伺服驱动器的速度控制指令

其中：k_eε＝150；u_maxsε＝3000，u_minsε＝-3000，此二者参数有高低电机额定转速决定；

若e_β≤10mil，计算稳定条件下的方位随动伺服驱动器的速度控制指令

其中：u_psβ＝17，u_isβ＝0.02；

若e_ε≤10mil，计算稳定条件下的高低随动伺服驱动器的速度控制指令

其中：u_psε＝15，u_isε＝0.02；

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

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

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

(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))

其中：给定的方位电机轴J_β＝0.0252(含负载)，其电机极对数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ε(j)＝e_εω(j)+γ_ε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)

其中：给定的高低电机轴J_ε＝0.011(含负载)，其极对数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)将高低电流指令

和方位电流指令

分别发送给高低和方位电流环。

所用参数的范围见表1。

表1参数取值范围

Claims (8)

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)将方位驱动的电流指令

和高低驱动的电流指令

分别发送给高低和方位电流环。

2.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的航向角和俯仰角的带阻滤波：

ψ′(i)＝b₁₀ψ(i)+b₁₁ψ(i-1)+b₁₂ψ(i-2)-a₁₁ψ′(i-1)-a₁₂ψ′(i-1)

θ′(i)＝b₂₀θ(i)+b₂₁θ(i-1)+b₂₂θ(i-2)-a₂₁θ′(i-1)-a₂₂θ′(i-1)

其中：b₁₀，b₁₁，b₁₂，a₁₁，a₁₂,为航向角滤波系数；b₂₀，b₂₁，b₂₂，a₂₁，a₂₂为俯仰角滤波系数；ψ′(i)为航向角ψ(i)滤波值；θ′(i)为俯仰角θ(i)滤波值；

其中T为位置采样周期；ω_n为谐振中心频率；k₁为陷波器深度系数，k₂为陷波器宽度系数。

3.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的方位位置控制误差e_β(i)＝ψ_ref(i)-ψ′(i)，高低位置控制误差e_ε(i)＝θ_ref(i)-θ′(i)。

4.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的方位位置和高低位置控制误差的设定阈值小于3mil/mrad。

5.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的大幅调转下的方位随动伺服驱动器的速度控制指令

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

所述的大幅调转下的高低随动伺服驱动器的速度控制指令

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

所述的稳定条件下的方位随动伺服驱动器的速度控制指令

其中，

为方位位置PI控制量，K_psβ和K_isβ为其PI比例控制系数和积分系数；

所述的稳定条件下的高低随动伺服驱动器的速度控制指令；

其中：

为高低位置PI控制量，K_psε和K_isε为其PI比例控制系数和积分系数。

6.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的高低补偿角速率

方位干扰补偿角速率

所述的高低干扰滤校正量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分别为高低和方位增益常数；

所述的高低伺服总的速度指令

方位伺服驱动总的速度指令

7.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的方位驱动的电流指令

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)

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_β方位系统综合粘性摩擦系数；ω_β(j)为方位电机转速；γ_β,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β分别为状态估计的中间变量。

8.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法，其特征在于，所述的高低驱动的电流指令

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[-λ_1lεsgn(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)

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_ε高低系统综合粘性摩擦系数；ω_ε(j)为高低电机转速；γ_ε,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ε分别为状态估计的中间变量。

Images