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

Abstract

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

Description

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

技术领域

本发明属于光学跟踪平台伺服控制***设计及应用领域，具体涉及一种适用于滚转-摆动结构成像导引头的，滚摆式导引头跟踪回路角增量优化方法。

背景技术

目前常用的两轴成像导引头稳定平台多采用俯仰-方位结构和滚转-摆动结构。俯仰-方位结构导引头通常以俯仰框架为内框，以方位框架为外框，其优点是简单紧凑，两通道相互独立、耦合少，控制及跟踪算法较为成熟；不足之处是由于空间结构的约束以及大离轴角的盲区的限制，这种平台的离轴角很难突破±60°。滚转-摆动结构形式的导引头稳定平台以滚转框架为外框，以摆动框架为内框。摆动方向的离轴角可以达到±90°，滚转方向则可以达到0～360°，如果使用滑环，还能够实现连续旋转，很容易覆盖整个头罩前半球空间范围，可以实现对目标的大离轴角探测与跟踪。这种滚转-摆动式平台结构能同时满足对导引头体积小、质量轻和离轴角大等多项要求。但是，滚转-摆动式导引头的不足之处是跟踪回路的控制角位置指令提取及优化存在一定的困难。

在滚转-摆动式导引头中，图像处理器输出的脱靶量信息是直角坐标形式，而导引头的框架结构是极坐标形式，由直角坐标脱靶量到极坐标控制指令的解算过程中，可以有很多种途径，依据使用的输入量和求解函数不同主要分为三种方法。第一种方法是综合陀螺的角速度信号、探测器的俯仰和方位脱靶量、滚转和摆动角位置信息，分别使用反正切和反余弦函数求解滚转和俯仰角增量。第二种方法同样是综合陀螺的角速度信号、探测器的俯仰和方位脱靶量、滚转和摆动角位置信息，使用反正切函数求解滚转和俯仰角增量。第三种方法是综合探测器的俯仰和方位脱靶量、滚转和摆动角位置信息、弹目距离信息，使用Paden-Kahan子问题求解方法获得滚转和俯仰框架的角位置指令，并依据最小角增量准则优化控制指令。

上述三种方法中，前两种方法需要的输入信息较多，并且针对具体应用没有提出反三角函数数值多解指令优化方法；第三种方法虽然提出了指令优化准则，但是求解角增量过程中，除了要用到前两种方法中的信息，还需要用到弹目距离信息，在导引头测试和应用中都会造成***复杂和成本上升。

发明内容

本发明要解决现有技术中的技术问题，提出一种用于滚转-摆动式导引头角位置指令提取及优化的，滚摆式导引头跟踪回路角增量优化方法。

为减少摩擦和噪声，本发明所提出方法的工程背景为滚转框不应用滑环，且以零位为基准，顺时针和逆时针均可以旋转360°；摆动框架的离轴角范围为±90°。

为了解决上述技术问题，本发明的技术方案具体如下：

一种滚摆式导引头跟踪回路角增量优化方法，包括以下步骤：

当滚转框的相对运动范围绝对值小于等于π/2时，直接输出解算得到的滚转和摆动框的角位置指令；

当滚转框的相对运动范围绝对值为(π/2,3π/2]时，将摆动框的角位置指令取负号，然后控制滚转框的相对运动范围绝对值小于π/2；

当滚转框的相对运动范围绝对值为(3π/2,2π)时，示滚转框的限位情况优化输出指令。

在上述技术方案中，应用该方法时：

在失调角度比较小的情况下，滚转和摆动框架的控制角增量均可以用反正切函数来表示，

$\left\{\begin{array}{cc}\mathrm{\Δ}{\mathrm{\θ}}_r=\arctan \left(\frac{\mathrm{\Δz}}{\mathrm{\Δy}}\right)& \mathrm{\Δ}{\mathrm{\θ}}_r\∈[-\mathrm{\π}/2,\mathrm{\π}/2]\\ \mathrm{\Δ}{\mathrm{\θ}}_p=\mathrm{acr}\tan \left(\frac{\sqrt{{(\mathrm{\Δz}\·\mathrm{\μ})}^2+{(\mathrm{\Δy}\·\mathrm{\μ})}^2}}{f}\right)& \mathrm{\Δ}{\mathrm{\θ}}_p\∈[0,\mathrm{\π}/2]\end{array}\right.---\left(1\right)$

其中，f表示光学***的焦距，单位是mm；Δθ_r表示滚转框的角位置增量，单位是rad；Δθ_p表示摆动框的角位置增量，单位是rad；

该方法具体包括以下步骤：

步骤i：按照式(1)的表示求出滚转和摆动角增量在[-π/2,π/2]值域内的表达式；

步骤ii：将滚转框架的角增量变换为[0,2π)值域内的表示，将摆动框架的角增量变换为[0,π/2]值域范围内的表示，将图像探测器输出的脱靶量转化为以滚转框和摆动框零位为起点的绝对角位置指令；

步骤iii：对控制指令进行分区，划分出多个子区域；

步骤iv：在每个子区域内分别优化输出的控制指令。

在上述技术方案中，所述步骤ii中，将摆动框架的角增量变换为[0,π/2]值域范围内的表示的表达式为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{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]\mathrm{\&pi;}& (\mathrm{\&Delta;y}>0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;y}<0,\mathrm{\&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]+\begin{array}{}\end{array}\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}>0,\mathrm{\&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]& (\mathrm{\&Delta;y}<0,\mathrm{\&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]& (\mathrm{\&Delta;y}=0,\mathrm{\&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-\mathrm{sign}\left(\mathrm{\&Delta;z}\right)/2\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}=0,\mathrm{\&Delta;z}\&NotEqual;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-\mathrm{sign}\left(\mathrm{\&Delta;y}\right)/2\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}\&NotEqual;0,\mathrm{\&Delta;z}=0)\end{array}\right.---\left(2\right).$

在上述技术方案中，所述步骤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)和其它。

在上述技术方案中，所述步骤iv具体为：

子区域I中指令的优化输出为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}0,\mathrm{\&pi;}/2)\right.---\left(6\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}\mathrm{\&pi;\&pi;}/2,\mathrm{\&pi;})\right.---\left(7\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}\mathrm{\&pi;},3\mathrm{\&pi;}/2)\right.---\left(8\right);$

子区域II中指令的优化输出为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\left[-3\mathrm{\&pi;}/2,-\mathrm{\&pi;}/2\right)& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}3\mathrm{\&pi;}/2,2\mathrm{\&pi;})\right.---\left(12\right).$

本发明具有以下的有益效果：

本发明的滚摆式导引头跟踪回路角增量优化方法，是结合滚转-摆动式导引头的具体应用提出的一种角位置指令优化算法。相比现有技术中的其他方法，本发明的滚摆式导引头跟踪回路角增量优化方法所使用的输入量少且容易获得，求解角增量的表达式简单且公式计算量小。

本发明的滚摆式导引头跟踪回路角增量优化方法，能够有效地减小滚转框指令的绝对值，节省了***的总能量。

本发明的滚摆式导引头跟踪回路角增量优化方法，给出的角位置指令在导引头的跟踪范围内具有明确、单一的弹目指向关系，逻辑关系清晰，并且求解过程不需要大量的矩阵运算，硬件负担轻，实时性好。

附图说明

图1是本发明位置回路指令提取及优化的总体方案图；

图2是本发明绝对角位置指令提取方法流程图；

图3是本发明角位置指令优化方案总体流程图；

图4是本发明绝对角位置指令在第一象限时指令优化示意图；

图5是本发明绝对角位置指令在第二象限时指令优化示意图；

图6是本发明绝对角位置指令在第三象限时指令优化示意图；

图7是本发明绝对角位置指令在第四象限时指令优化示意图。

具体实施方式

本发明的发明思想为：

滚转-摆动式导引头图像探测器常用的靶面形状有矩形和圆形，靶心位于靶面的几何中心，目标在靶面直角坐标系中的坐标值的像素表示即为脱靶量。按照导弹的坐标定义***和竖直脱靶量分别规定为Δy和Δz，单位是像元数。探测器的像元尺寸表示为μ，单位是mm。在失调角度比较小的情况下，滚转和摆动框架的控制角增量均可以用反正切函数来表示，

$\left\{\begin{array}{cc}{\mathrm{\&Delta;\&theta;}}_r=\arctan \left(\frac{\mathrm{\&Delta;z}}{\mathrm{\&Delta;y}}\right)& \mathrm{\&Delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]\\ {\mathrm{\&Delta;\&theta;}}_p=\arctan \left(\frac{\sqrt{{(\mathrm{\&Delta;z}\&CenterDot;\mathrm{\&mu;})}^2+{(\mathrm{\&Delta;y}\&CenterDot;\mathrm{\&mu;})}^2}}{f}\right)& {\mathrm{\&Delta;\&theta;}}_p\&Element;[0,\mathrm{\&pi;}/2]\end{array}\right.---\left(1\right)$

其中，f表示光学***的焦距，单位是mm；Δθ_r表示滚转框的角位置增量，单位是rad；Δθ_p表示摆动框的角位置增量，单位是rad。

反正切函数的值域为[-π/2,π/2]，对于摆动框而言，离轴角与弹目夹角的摆动轴分量有明确的一一对应关系；而对于滚转框而言，由于离轴角范围为[-2π,2π]，离轴角与弹目夹角的滚转轴分量存在数值多解问题，加之滚转框架不能连续旋转，需要设计相应的角位置指令优化算法。

优化算法的设计目的为减少滚转框的运动角增量，保证滚转框的相对运动范围为[-π/2,π/2]，这样可以避免***中大转动惯量部分的大阶跃响应，从而减小振荡，降低***的功耗。算法的基本思想是当滚转框的相对运动范围绝对值小于等于π/2时，直接输出解算得到的滚转和摆动框的角位置指令；当滚转框的相对运动范围绝对值为(π/2,3π/2]时，将摆动框的角位置指令取负号，然后控制滚转框的相对运动范围绝对值小于π/2；当滚转框的相对运动范围绝对值为(3π/2,2π)时，示滚转框的限位情况优化输出指令。优化算法的输入量为滚转和摆动框的当前角位置θ_r和θ_p，滚转和摆动框架角增量Δθ_r和Δθ_p，输出指令为滚转和摆动框的角位置指令θ_rc和θ_pc。优化算法的程序流程框图如图1所示。图中，将滚转框的角位置和解算出的角增量定义在第一至第四象限中，按照滚转角增量的限制条件，输出两个框架的角位置指令，控制导引头指向目标。

下面结合附图对本发明做以详细说明。

1、按照式(1)的表示求出滚转和摆动角增量在[-π/2,π/2]值域内的表达式。

2、将滚转框架的角增量变换为[0,2π)值域内的表示，将摆动框架的角增量变换为[0,π/2]值域范围内的表示，通过这样的变换，将图像探测器输出的脱靶量转化为以滚转框和摆动框零位为起点的绝对角位置指令。变换流程如图2所示。按照图中的方法，该变换的解析表达如式(2)所示。

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{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]\mathrm{\&pi;}& (\mathrm{\&Delta;y}>0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;y}<0,\mathrm{\&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]+\begin{array}{}\end{array}\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}>0,\mathrm{\&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]& (\mathrm{\&Delta;y}<0,\mathrm{\&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]& (\mathrm{\&Delta;y}=0,\mathrm{\&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-\mathrm{sign}\left(\mathrm{\&Delta;z}\right)/2\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}=0,\mathrm{\&Delta;z}\&NotEqual;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-\mathrm{sign}\left(\mathrm{\&Delta;y}\right)/2\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;y}\&NotEqual;0,\mathrm{\&Delta;z}=0)\end{array}\right.---\left(2\right)$

使用表达式(2)计算的角位置指令，可以实现对导引头离轴角范围内任意目标的跟踪，但是输出指令的超调量和精度与导引头的当前框架角有关，若指令的阶跃值过大，会造成***振荡，跟踪精度下降。针对这个问题，接下来提出了分区域指令优化方法。

3、按照图3的方法,首先对控制指令进行分区。指令的分区优化有两级逻辑判断依据。在第一级逻辑判断中，判定条件为滚转框的当前角位置θ_r，在[0,2π)范围内将θ_r按照象限的定义分为四个子区域。如图3中所示，在不同的象限中，根据滚转框架最小角增量的控制原则，执行第二级逻辑判断。第二级逻辑判断的判定条件为滚转框架的角增量值，如图中所示，将由公式(2)解算出的绝对角位置指令与当前角位置相减即为判定条件。在第二级逻辑判定中共有两个部分，其中第一级分区中的前三个象限为子区域I，第一级分区中的第四象限为子区域II。在子区域I中，第二级逻辑判断的条件相同，如图3中所示，分别为δθ_r∈[-π/2,π/2]、δθr∈(π/2,3π/2]和其它。在δθ_r的每个分区内，采取不同的控制策略。同样的，在子区域II中，第二级逻辑判断的条件为δθ_r∈[-π/2,π/2]、δθ_r∈[-3π/2,-π/2)和其它，不同分区内采取相应的控制策略减小滚转角增量。在第二级逻辑判断子区域内，控制策略分为子区域I和II的原因是本算法适用的工程条件是滚转框架离轴角为±360°且无法连续旋转。

4、实现分区以后，在每个子区域内分别优化输出的控制指令，首先说明子区域I的优化过程。

以θ_r∈[0,π/2)角度范围为例，如图4(a)中所示。若δθ_r∈[-π/2,π/2]，则滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(3\right)$

式(3)输出指令的优化方法对应图3中的指令优化A。θ_r和θ_ri的取值范围对应图4(b)所示。

若δθ_r∈(π/2,3π/2]，则滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}=({\mathrm{\&theta;}}_{\mathrm{ri}}-\mathrm{\&pi;})-{\mathrm{\&theta;}}_r={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r-\mathrm{\&pi;}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={-\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(4\right)$

式(4)输出指令的优化方法对应图3中的指令优化B。θ_r和θ_ri的取值范围对应图4(c)所示。

在δθ_r取值为其它范围时，滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r-2\mathrm{\&pi;}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(5\right)$

式(5)输出指令的优化方法对应图3中的指令优化C。θ_r和θ_ri的取值范围对应图4(d)所示。

子区域I内的其它方案的优化过程与θ_r∈[0,π/2)的优化方法相同，θ_r∈[π/2,π)和θ_r∈[π,3π/2)的优化过程的图示表达分别见图5和图6。下面给出I子区域内所有指令的优化公式，

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}0,\mathrm{\&pi;}/2)\right.---\left(6\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}\mathrm{\&pi;}/2,\mathrm{\&pi;})\right.---\left(7\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}-1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/\mathrm{2,3}\mathrm{\&pi;}/2]& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}\mathrm{\&pi;},3\mathrm{\&pi;}/2)\right.---\left(8\right)$

接下来说明子区域II中指令的优化过程。在子区域II中θ_r∈[3π/2,2π),如图7(a)中所示。若δθ_r∈[-π/2,π/2]，则滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(9\right)$

式(9)输出指令的优化方法对应图3中的指令优化D。θ_r和θ_ri的取值范围对应图7(b)所示。

若δθ_r∈[-3π/2,-π/2)，则滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}=({\mathrm{\&theta;}}_{\mathrm{ri}}+\mathrm{\&pi;})-{\mathrm{\&theta;}}_r={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r+\mathrm{\&pi;}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={-\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(10\right)$

式(10)输出指令的优化方法对应图3中的指令优化E。θ_r和θ_ri的取值范围对应图7(c)所示。

若δθ_r取值为其它范围时，则滚转框和摆动框的角增量指令为，

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}=({\mathrm{\&theta;}}_{\mathrm{ri}}+2\mathrm{\&pi;})-{\mathrm{\&theta;}}_r={\mathrm{\&theta;}}_{\mathrm{ri}}-{\mathrm{\&theta;}}_r+2\mathrm{\&pi;}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}={\mathrm{\&theta;}}_{\mathrm{pi}}-{\mathrm{\&theta;}}_p\end{array}\right.---\left(11\right)$

式(11)输出指令的优化方法对应图3中的指令优化F。θ_r和θ_ri的取值范围对应图7(d)所示。

子区域II中指令的优化输出为，

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{rc}}\\ {\mathrm{\&theta;}}_{\mathrm{pc}}\end{array}\right]=\left\{\begin{array}{ccc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& \mathrm{\&delta;}{\mathrm{\&theta;}}_r\&Element;[-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2]& \\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}1\\ 0\end{array}\right]\end{array}\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\left[-3\mathrm{\&pi;}/2,-\mathrm{\&pi;}/2\right)& {\mathrm{\&theta;}}_r\&Element;[\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{ri}}\\ {\mathrm{\&theta;}}_{\mathrm{pi}}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\mathrm{else}& \end{array}3\mathrm{\&pi;}/2,2\mathrm{\&pi;})\right.---\left(12\right)$

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (1)

1.一种滚摆式导引头跟踪回路角增量优化方法，其特征在于，包括以下步骤：

当滚转框架的相对当前角位置运动范围绝对值小于等于π/2时，直接输出解算得到的滚转和摆动框架的角位置指令；

当滚转框架的相对当前角位置运动范围绝对值为(π/2,3π/2]时，将摆动框架的角位置指令取负号，然后控制滚转框架的相对当前角位置运动范围绝对值小于π/2；

当滚转框架的相对运动范围绝对值为(3π/2,2π)时，视滚转框架的限位情况优化输出指令，同样保证滚转框架相对当前角位置的运动范围绝对值小于π/2；

应用该方法时：

在失调角度比较小的情况下，滚转框架和摆动框架的控制角增量均可以用反正切函数来表示，

$\left\{\begin{array}{cc}{\mathrm{\&Delta;\&theta;}}_r=arctan\left(\frac{\mathrm{\&Delta;}z}{\mathrm{\&Delta;}y}\right)& {\mathrm{\&Delta;\&theta;}}_r\&Element;\&lsqb;-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2\&rsqb;\\ {\mathrm{\&Delta;\&theta;}}_p=arctan\left(\frac{\sqrt{{(\mathrm{\&Delta;}z\&CenterDot;\mathrm{\&mu;})}^2+{(\mathrm{\&Delta;}y\&CenterDot;\mathrm{\&mu;})}^2}}{f}\right)& {\mathrm{\&Delta;\&theta;}}_p\&Element;\&lsqb;0,\mathrm{\&pi;}/2\&rsqb;\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}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;}y>0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;}y<0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;}y>0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;}y<0,\mathrm{\&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]\mathrm{\&pi;}& (\mathrm{\&Delta;}y=0,\mathrm{\&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(\mathrm{\&Delta;}z\right)\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;}y=0,\mathrm{\&Delta;}z\&NotEqual;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(\mathrm{\&Delta;}y/2)\\ 0\end{array}\right]\mathrm{\&pi;}& (\mathrm{\&Delta;}y\&NotEqual;0,\mathrm{\&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}{\mathrm{\&theta;}}_{rc}\\ {\mathrm{\&theta;}}_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\&lsqb;-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/2,3\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;else\end{array},{\mathrm{\&theta;}}_r\&Element;\&lsqb;0,\mathrm{\&pi;}/2)---\left(6\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{rc}\\ {\mathrm{\&theta;}}_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\&lsqb;-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/2,3\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;else\end{array},{\mathrm{\&theta;}}_r\&Element;\&lsqb;\mathrm{\&pi;}/2,\mathrm{\&pi;})---\left(7\right)$

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{rc}\\ {\mathrm{\&theta;}}_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\&lsqb;-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}-1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(\mathrm{\&pi;}/2,3\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;else\end{array},{\mathrm{\&theta;}}_r\&Element;\&lsqb;\mathrm{\&pi;},3\mathrm{\&pi;}/2)---\left(8\right);$

子区域II中指令的优化输出为：

$\left[\begin{array}{c}{\mathrm{\&theta;}}_{rc}\\ {\mathrm{\&theta;}}_{pc}\end{array}\right]=\{\begin{array}{cc}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;\&lsqb;-\mathrm{\&pi;}/2,\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;(-3\mathrm{\&pi;}/2,-\mathrm{\&pi;}/2\&rsqb;\\ \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_{ri}\\ {\mathrm{\&theta;}}_{pi}\end{array}\right]+\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\&theta;}}_r\\ {\mathrm{\&theta;}}_p\end{array}\right]+\left[\begin{array}{c}2\\ 0\end{array}\right]\mathrm{\&pi;}& {\mathrm{\&delta;\&theta;}}_r\&Element;else\end{array},{\mathrm{\&theta;}}_r\&Element;\&lsqb;3\mathrm{\&pi;}/2,2\mathrm{\&pi;})---\left(12\right);$

其中，θ_rc为用于控制滚转框架的最终优化完成的滚转框架的角增量指令，θ_pc为用于控制滚转框架的最终优化完成的摆动框架的角增量指令。