一种滚摆式导引头跟踪回路角增量优化方法

摘要

本发明涉及一种滚摆式导引头跟踪回路角增量优化方法，包括以下步骤：当滚转框的相对运动范围绝对值小于等于π/2时，直接输出解算得到的滚转和摆动框的角位置指令；当滚转框的相对运动范围绝对值为(π/2,3π/2]时，将摆动框的角位置指令取负号，然后控制滚转框的相对运动范围绝对值小于π/2；当滚转框的相对运动范围绝对值为(3π/2,2π)时，示滚转框的限位情况优化输出指令。本发明的滚摆式导引头跟踪回路角增量优化方法，给出的角位置指令在导引头的跟踪范围内具有明确、单一的弹目指向关系，逻辑关系清晰，并且求解过程不需要大量的矩阵运算，硬件负担轻，实时性好。

主权项

一种滚摆式导引头跟踪回路角增量优化方法，其特征在于，包括以下步骤：当滚转框架的相对当前角位置运动范围绝对值小于等于π/2时，直接输出解算得到的滚转和摆动框架的角位置指令；当滚转框架的相对当前角位置运动范围绝对值为(π/2,3π/2]时，将摆动框架的角位置指令取负号，然后控制滚转框架的相对当前角位置运动范围绝对值小于π/2；当滚转框架的相对运动范围绝对值为(3π/2,2π)时，视滚转框架的限位情况优化输出指令，同样保证滚转框架相对当前角位置的运动范围绝对值小于π/2；应用该方法时：在失调角度比较小的情况下，滚转框架和摆动框架的控制角增量均可以用反正切函数来表示，$\left\{\begin{array}{cc}{\mathrm{\Delta \theta }}_r=arctan\left(\frac{\Delta z}{\Delta y}\right)& {\mathrm{\Delta \theta }}_r\in [-\pi /2,\pi /2]\\ {\mathrm{\Delta \theta }}_p=arctan\left(\frac{\sqrt{{(\Delta z·\mu )}^2+{(\Delta y·\mu )}^2}}{f}\right)& {\mathrm{\Delta \theta }}_p\in [0,\pi /2]\end{array}\right.---\left(1\right)$其中，f表示光学系统的焦距，单位是mm；Δθ_r表示滚转框架的控制角增量，单位是rad；Δθ_p表示摆动框架的控制角增量，单位是rad；μ为像元尺寸，单位是mm；⊿y为目标在靶面上水平方向的脱靶量，⊿z为目标在靶面上竖直方向的脱靶量；该方法具体包括以下步骤：步骤i：按照式(1)的表示求出滚转框架和摆动框架角增量在[‑π/2,π/2]值域内的表达式；步骤ii：将滚转框架的角增量变换为[0,2π)值域内的表示，将摆动框架的角增量变换为[0,π/2]值域范围内的表示，将图像探测器输出的脱靶量转化为以滚转框架和摆动框架零位为起点的绝对角位置指令；步骤iii：对绝对角位置指令进行分区，划分出多个子区域；步骤iv：在每个子区域内分别优化输出的角增量指令；所述步骤ii中，将摆动框架的角增量变换为[0,π/2]值域范围内的表示的表达式为：$\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]=\left\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\pi & (\Delta y>0,\Delta z>0)\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & (\Delta y<0,\Delta z<0)\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\pi & (\Delta y>0,\Delta z<0)\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}2\\ 0\end{array}\right]\pi & (\Delta y<0,\Delta z>0)\\ \left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & (\Delta y=0,\Delta z=0)\\ \left[\begin{array}{cc}0& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}1-sign\left(\Delta z\right)\\ 0\end{array}\right]\pi & (\Delta y=0,\Delta z\ne 0)\\ \left[\begin{array}{cc}0& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Delta \theta }}_r\\ {\mathrm{\Delta \theta }}_p\end{array}\right]+\left[\begin{array}{c}1/2-sign(\Delta y/2)\\ 0\end{array}\right]\pi & (\Delta y\ne 0,\Delta z=0)\end{array}\right.---\left(2\right);$式(2)中，θ_pi为摆动框架的角增量变换为[0,π/2]值域范围内摆动框架的绝对角位置指令值，θ_ri为摆动框架的角增量变换为[0,π/2]值域范围内滚转框架的绝对角位置指令值；所述步骤iii的对绝对角位置指令进行分区具体为两级逻辑判断：在第一级逻辑判断中，判定条件为滚转框架的当前角位置θ_r，在[0,2π)范围内将θ_r按照象限的定义分为四个子区域；在第二级逻辑判断中，判定条件为滚转框架的角增量值，由公式(2)解算出的绝对角位置指令与当前角位置相减即为判定条件；所述步骤iii中，第二级逻辑判定中共有两个部分：第一级分区中的前三个象限为子区域I，第一级分区中的第四象限为子区域II；在子区域I中，第二级逻辑判断的条件分别为δθ_r∈[‑π/2,π/2]、δθ_r∈(π/2,3π/2]和其它；在子区域II中，第二级逻辑判断的条件分别为δθ_r∈[‑π/2,π/2]、δθ_r∈[‑3π/2,‑π/2)和其它，其中，δθ_r为滚转框架绝对角位置指令θ_ri与滚转框架当前角位置θ_r相减；所述步骤iv具体为：子区域I中指令的优化输出为：$\left[\begin{array}{c}{\theta }_{rc}\\ {\theta }_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in [-\pi /2,\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in (\pi /2,3\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}-2\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in else\end{array},{\theta }_r\in [0,\pi /2)---\left(6\right)$$\left[\begin{array}{c}{\theta }_{rc}\\ {\theta }_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in [-\pi /2,\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in (\pi /2,3\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in else\end{array},{\theta }_r\in [\pi /2,\pi )---\left(7\right)$$\left[\begin{array}{c}{\theta }_{rc}\\ {\theta }_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in [-\pi /2,\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in (\pi /2,3\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in else\end{array},{\theta }_r\in [\pi ,3\pi /2)---\left(8\right);$子区域II中指令的优化输出为：$\left[\begin{array}{c}{\theta }_{rc}\\ {\theta }_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in [-\pi /2,\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in (-3\pi /2,-\pi /2]\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\theta }_{ri}\\ {\theta }_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\theta }_r\\ {\theta }_p\end{array}\right]+\left[\begin{array}{c}2\\ 0\end{array}\right]\pi & {\mathrm{\delta \theta }}_r\in else\end{array},{\theta }_r\in [3\pi /2,2\pi )---\left(12\right);$其中，θ_rc为用于控制滚转框架的最终优化完成的滚转框架的角增量指令，θ_pc为用于控制滚转框架的最终优化完成的摆动框架的角增量指令。