一种基座运动引起台体漂移的角速率计算方法

摘要

本发明公开了一种基座运动引起台体漂移的角速率计算方法，该方法应用于双轴惯性平台系统，包括：获取台体在XP、YP和ZP轴上的三个陀螺仪输出值；测量得到所述双轴惯性平台系统的内部相对转动角度；将陀螺仪的输出值和测量得到的内部相对转动角度作为伺服回路的输入值，得到解耦输出值，以使控制器根据所述解耦输出值控制伺服回路稳定工作；当所述伺服回路稳定工作时，根据与所述双轴惯性平台系统共基座的外部角速率传感器，测量得到基座角速率；根据测量得到基座的角速率和内部相对转动角度进行解算，得到漂移角速率分量。本发明所述的方法在任意框架角情况下都无奇异值，可以有效提高载体无轨迹约束条件下的全姿态适应能力。

权利要求

1.一种基座运动引起台体漂移的角速率计算方法，其特征在于，所述方法应用于双轴惯性平台系统，所述双轴惯性平台系统的伺服回路采用平面坐标分解器进行解耦，所述双轴惯性平台系统包括基座、框架和台体，其中，基座本体坐标系为X1Y1Z1、框架本体坐标系为XP1YP1ZP1，台体本体坐标系为XPYPZP；三个本体坐标系的原点重合，并且：台体本体坐标系的ZP轴与框架本体坐标系的ZP1轴重合，基座本体坐标系的Y1轴与框架本体坐标系的YP1轴重合；其中，基座与载体固连，当双轴惯性平台系统在载体带动下发生内部相对转动时，基座绕框架本体坐标系的YP1轴转动，框架绕台体本体坐标系的ZP轴转动；

其中，所述基座运动引起台体漂移的角速率计算方法包括：

获取台体在XP、YP和ZP轴上的三个陀螺仪输出值uix、uiy和uiz；

测量得到所述双轴惯性平台系统的内部相对转动角度；其中，所述内部相对转动角度，包括：基座绕框架本体坐标系的YP1轴转动的角度βyk和框架绕台体本体坐标系的ZP轴转动的角度βzk；

将uix、uiy、uiz和βzk作为伺服回路的输入值，得到解耦输出值，以使控制器根据所述解耦输出值控制伺服回路稳定工作；

当所述伺服回路稳定工作时，根据与所述双轴惯性平台系统共基座的外部角速率传感器，测量得到基座的X1轴角速率ωx1和Z1轴角速率ωz1；

根据如下公式分别计算台体在XP轴、YP轴和ZP轴上的漂移角速率分量ωxp、ωyp和ωzp：其中，解耦输出值为uoz和uoy；其中，uoz＝uiz，uoy＝uiycosβzk-uixsinβzk；

设定条件如下：基座绕框架本体坐标系的YP1轴转动的角度βyk＝0°；框架绕台体本体坐标系的ZP轴转动的角度βzk＝0°；则，可得：uoz＝uiz、uoy＝uiy、ωxp＝ωx1、ωyp＝0、ωzp＝0；

设定条件如下：基座绕框架本体坐标系的YP1轴转动的角度βyk＝0°；框架绕台体本体坐标系的ZP轴转动的角度βzk＝90°，则，可得：uoz＝uiz、uoy＝-uix、ωxp＝ωz1、ωyp＝ωy1、ωzp＝

0。

2.根据权利要求1所述的基座运动引起台体漂移的角速率计算方法，其特征在于，所述测量得到所述双轴惯性平台系统的内部相对转动角度，包括：通过安装在框架的YP1轴上的角度传感器，测量得到基座绕框架本体坐标系的YP1轴转动的角度βyk；

通过安装在台体的ZP轴上的角度传感器，测量得到框架绕台体本体坐标系的ZP轴转动的角度βzk。

3.根据权利要求1或2所述的基座运动引起台体漂移的角速率计算方法，其特征在于，转动角度βyk和βzk的取值范围为0～360°。

说明书

一种基座运动引起台体漂移的角速率计算方法

技术领域

[0001] 本发明属于双轴惯性平台系统技术领域，尤其涉及一种基座运动引起台体漂移的角速率计算方法。

背景技术

[0002] 双轴惯性平台系统已广泛地用于车载定位定向或导引头位标器中。在车载定位定向中应用时，双轴惯性平台系统主要用于隔离载体的两个轴向的角运动。比如，实现对俯仰角和横滚角的隔离，也可用于对俯仰角和方位角的隔离。在车载应用条件下，由于机动运动不大，因此台体轴和框架轴之间没有实现解耦，故，台体轴的转角必须为小角度。

[0003] 随着战术导弹对惯性平台系统小体积的要求，采用四轴惯性平台系统或三轴惯性平台系统已不能满足要求，为此，体积较小的双轴惯性平台系统被采用，双轴惯性平台系统的优点在于体积小、同时隔离两个自由度的角运动。

[0004] 然而，由于有大机动全姿态的要求，双轴惯性平台系统的现有两条伺服回路不解耦的措施逐渐难以满足实际需求。

发明内容

[0005] 本发明的技术解决问题：克服现有技术的不足，提供一种基座运动引起台体漂移的角速率计算方法，在任意框架角情况下都无奇异值，可以有效提高载体无轨迹约束条件下的全姿态适应能力。

[0006] 为了解决上述技术问题，本发明公开了一种基座运动引起台体漂移的角速率计算方法，所述方法应用于双轴惯性平台系统，所述双轴惯性平台系统的伺服回路采用平面坐标分解器进行解耦，所述双轴惯性平台系统包括基座、框架和台体，其中，基座本体坐标系为X1Y1Z1、框架本体坐标系为XP1YP1ZP1，台体本体坐标系为XPYPZP；三个本体坐标系的原点重合，并且：台体本体坐标系的ZP轴与框架本体坐标系的ZP1轴重合，基座本体坐标系的Y1轴与框架本体坐标系的YP1轴重合；其中，基座与载体固连，当双轴惯性平台系统在载体带动下发生内部相对转动时，基座绕框架本体坐标系的YP1轴转动，框架绕台体本体坐标系的ZP轴转动；

[0007] 其中，所述基座运动引起台体漂移的角速率计算方法包括：

[0008] 获取台体在XP、YP和ZP轴上的三个陀螺仪输出值uix、uiy和uiz；

[0009] 测量得到所述双轴惯性平台系统的内部相对转动角度；其中，所述内部相对转动角度，包括：基座绕框架本体坐标系的YP1轴转动的角度βyk和框架绕台体本体坐标系的ZP轴转动的角度βzk；

[0010] 将uix、uiy、uiz和βzk作为伺服回路的输入值，得到解耦输出值，以使控制器根据所述解耦输出值控制伺服回路稳定工作；

[0011] 当所述伺服回路稳定工作时，根据与所述双轴惯性平台系统共基座的外部角速率传感器，测量得到基座的X1轴角速率ωx1和Z1轴角速率ωz1；

[0012] 根据如下公式分别计算台体在XP轴、YP轴和ZP轴上的漂移角速率分量ωxp、ωyp和ωzp：

[0013]

[0014] 在上述基座运动引起台体漂移的角速率计算方法中，所述测量得到所述双轴惯性平台系统的内部相对转动角度，包括：

[0015] 通过安装在框架的YP1轴上的角度传感器，测量得到基座绕框架本体坐标系的YP1轴转动的角度βyk；

[0016] 通过安装在台体ZP轴上的角度传感器，测量得到框架绕台体本体坐标系的ZP轴转动的角度βzk。

[0017] 在上述基座运动引起台体漂移的角速率计算方法中，转动角度βyk和βzk的取值范围为0～360°。

[0018] 本发明具有以下优点：

[0019] 本发明所述的基座运动引起台体漂移的角速率计算方法，给出了两个姿态角在任意象限时的漂移角定量计算方法，克服了现有技术中必须工作在小角度的限制条件。其次，本发明给出了漂移角速率的定量计算方法，使伺服回路不再受限于光学陀螺仪等速率陀螺的要求，本方法也可适用于动调陀螺仪、液浮陀螺仪、三浮陀螺仪等不具备直接输出角速率的惯性仪表作为伺服回路敏感元件。

附图说明

[0020] 图1是本发明实施例中一种双轴惯性平台系统的坐标系定义示意图；

[0021] 图2是本发明实施例中一种伺服回路的简化示意图；

[0022] 图3是本发明实施例中一种基座运动引起台体漂移的角速率计算方法的步骤流程图；

[0023] 图4是本发明实施例中一种基座运动引起台体漂移的角速率计算方法在工程应用中的解耦和计算原理框图。

具体实施方式

[0024] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公共的实施方式作进一步详细描述。

[0025] 本发明所述的基座运动引起台体漂移的角速率计算方法，以正交安装于双轴惯性平台系统台体的三个陀螺仪的输出值uix、uiy和uiz作为解耦环节的输入信息，通过信息融合后输出两个分别作用到台体轴和框架轴的轴端力矩电机，同时，以基座角速率作为输入信息，给出台体相对惯性空间的漂移角速率。

[0026] 在本发明实施例中，所述基座运动引起台体漂移的角速率计算方法主要应用于双轴惯性平台系统；其中，所述双轴惯性平台系统包括基座、框架和台体，其中，基座本体坐标系为X1Y1Z1、框架本体坐标系为XP1YP1ZP1，台体本体坐标系为XPYPZP；三个本体坐标系的原点重合，并且：台体本体坐标系的ZP轴与框架本体坐标系的ZP1轴重合，基座本体坐标系的Y1轴与框架本体坐标系的YP1轴重合；其中，基座与载体固连，当双轴惯性平台系统在载体带动下发生内部相对转动时，基座绕框架本体坐标系的YP1轴转动，框架绕台体本体坐标系的ZP轴转动。

[0027] 参照图1，示出了本发明实施例中一种双轴惯性平台系统的坐标系定义示意图。图1描述了双轴惯性平台系统各本体坐标系之间的关系，其中， 表示框架相对台体的相对角速度， 表示基座(箭体)相对框架的相对角速度。

[0028] 当载体与框架绕台体轴ZP转过βzk角度时，有：

[0029]

[0030] 其中，ωxp、ωyp和ωzp分别表示台体绕XP轴、YP轴和ZP轴的漂移角速率分量；ωxp1、ωyp1和ωzp1分别表示载体与框架一起绕XP1、YP1和ZP1轴的绝对角速率。

[0031] 当载体绕框架本体坐标系的YP1轴转过βyk角度时，有：

[0032]

[0033] 其中，ωx1、ωy1和ωz1分别表示载体绕X1、Y1和Z1轴的绝对角速率。

[0034] 由此可见，双轴惯性平台系统的台体角速率、框架角速率与基座(载体)角速率之间的关系式如下：

[0035]

[0036] 参照图2，示出了本发明实施例中一种伺服回路的简化示意图，如图2，有：

[0037]

[0038]

[0039] 其中，Gxp(s)≈Gyp(s)≈Gzp(s)≈Gp(s)，Gyc(s)≈Gzc(s)≈Gc(s)，有：

[0040]

[0041] 基于上述描述，参照图3，示出了本发明实施例中一种基座运动引起台体漂移的角速率计算方法的步骤流程图。在本实施例中，所述基座运动引起台体漂移的角速率计算方法，包括：

[0042] 步骤101，获取台体在XP、YP和ZP轴上的三个陀螺仪输出值uix、uiy和uiz。

[0043] 步骤102，测量得到所述双轴惯性平台系统的内部相对转动角度。

[0044] 在本实施例中，所述内部相对转动角度包括：基座绕框架本体坐标系的YP1轴转动的角度βyk和框架绕台体本体坐标系的ZP轴转动的角度βzk。

[0045] 优选的，可以通过安装在框架的YP1轴上的角度传感器，测量得到基座绕框架本体坐标系的YP1轴转动的角度βyk；通过安装在台体ZP轴上的角度传感器，测量得到框架绕台体本体坐标系的ZP轴转动的角度βzk。其中，转动角度βyk和βzk的取值范围为0～360°(包括0°和360°)，即该方法适用于全姿态计算。

[0046] 步骤103，将uix、uiy、uiz和βzk作为伺服回路的输入值，得到解耦输出值，以使控制器根据所述解耦输出值控制伺服回路稳定工作。

[0047] 如图2，解耦输出值为uoz和uoy；其中，uoz＝uiz，uoy＝uiycosβzk-uixsinβzk。

[0048] 步骤104，当所述伺服回路稳定工作时，根据与所述双轴惯性平台系统共基座的外部角速率传感器，测量得到基座的X1轴角速率ωx1和Z1轴角速率ωz1。

[0049] 步骤105，根据如下公式分别计算台体在XP轴、YP轴和ZP轴上的漂移角速率分量ωxp、ωyp和ωzp：

[0050]

[0051] 结合上述实施例，下面结合所述基座运动引起台体漂移的角速率计算方法在实际应用中的情况进行说明。参照图4，示出了本发明实施例中一种基座运动引起台体漂移的角速率计算方法在工程应用中的解耦和计算原理框图。

[0052] 其一，利用本发明所述的方法进行台体漂移角速率计算，设定条件如下：基座绕框架本体坐标系的YP1轴转动的角度βyk＝0°；框架绕台体本体坐标系的ZP轴转动的角度βzk＝0°；则，根据本发明所述的方法可得：uoz＝uiz、uoy＝uiy、ωxp＝ωx1、ωyp＝0、ωzp＝0。

[0053] 可以看出，台体ZP轴和框架YP1轴控制器输入量分别与台体ZP轴和YP轴上的陀螺仪的测量值一致。此时，台体XP轴将随着基座X1轴转动。

[0054] 其二，利用本发明所述的方法进行台体漂移角速率计算，设定条件如下：基座绕框架本体坐标系的YP1轴转动的角度βyk＝0°；框架绕台体本体坐标系的ZP轴转动的角度βzk＝90°，则，根据本发明所述的方法可得：uoz＝uiz、uoy＝-uix、ωxp＝ωz1、ωyp＝ωy1、ωzp＝0。

[0055] 可以看出，台体ZP轴的控制器输入量与ZP轴上的陀螺仪的测量值一致，但框架轴YP1的控制器输入量与YP轴上的陀螺仪无关，而只取决于XP轴上的陀螺仪，且方向相反。此时，台体YP轴将随着基座X1轴转动。

[0056] 上述两个具体实施例可以验证本发明的计算方法正确。

[0057] 综上所述，本发明所述的基座运动引起台体漂移的角速率计算方法，给出了两个姿态角在任意象限时的漂移角定量计算方法，克服了现有技术中必须工作在小角度的限制条件。其次，本发明给出了漂移角速率的定量计算方法，使伺服回路不再受限于光学陀螺仪等速率陀螺的要求，本方法也可适用于动调陀螺仪、液浮陀螺仪、三浮陀螺仪等不具备直接输出角速率的惯性仪表作为伺服回路敏感元件。

[0058] 本说明中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。

[0059] 以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

[0060] 本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。