CN105021195A - 一种四轴惯性稳定平台系统的伺服回路解耦方法 - Google Patents

Abstract

本发明提供了一种四轴惯性稳定平台系统的伺服回路解耦方法，包括如下步骤：1、根据台体上安装的陀螺仪输出的角速度，得到台体在X_p轴、Y_p轴和Z_p轴上的角速度分量；2、测量得到四轴惯性稳定平台系统内部相对转动的角度和角速度；3、计算台体、内框架、外框架和随动框架的合成转动角速度。该方法可以在平台系统相对转动角度为任意值的情况下，实现无奇异值的伺服回路解耦计算，从而提高了载体无轨迹约束条件下的全姿态适应能力。

Description

一种四轴惯性稳定平台系统的伺服回路解耦方法

技术领域

本发明涉及惯性测量技术领域，特别涉及一种四轴惯性稳定平台系统的伺服回路解耦方法，主要用于航空、航天领域的全姿态高精度导航。

背景技术

由于三轴惯性平台系统存在“框架锁定”现象，难以满足载体大机动运动的要求，因此，产生了四轴惯性平台系统。四轴惯性平台系统相对三轴惯性平台系统，在台体、内框架和外框架的基础上增加了随动框架，随动框架处于平台外框架和基座之间。

传统的解决方法如下：随动回路信号来自于内框架角，采用正割分解器进行增益补偿。但该方法的缺点是在外框架角为90°时，存在奇异值。参见文献“四轴平台伺服系统建模研究，中国惯性技术学报Vol.10，No.5，2002年10月”，以及“四轴平台随动系统的模型分析与设计，导航与控制，2014年第4期”。

目前，解决该问题的办法主要为：在外框架角为90°时采用框架锁定，一旦通过该角度值则又恢复成原随动方案。参见文献“四轴平台外框架角±90°时运动特性仿真分析，导航与控制，2009年第2期”。但该方法的缺点是，在外框架角一直保持90°时，则四轴平台退化为三轴平台，仍然存在“框架锁定”现象，不能实现载体运动的全姿态功能。

总之，上述方法存在着以条件判断来克服奇异值的缺点，为此，需要研究一种无奇异值、不受轨迹限制的解耦方法。

下面介绍目前现有技术的研究情况：

首先，四轴惯性稳定平台系统的五个本体坐标系的定义如图1所示，从中可以看出各本体坐标系之间关系。在图1中，设为内框架相对台体的相对角速度，为外框架相对内框架的相对角速度，为为基座(箭体)相对外框架的相对角速度，为基座(箭体)相对随动框架的相对角速度。

设为台体(包括陀螺仪壳体)对X_p、Y_p、Z_p轴的转动惯量；为内框架对X_p1、Y_p1、Z_p1轴的转动惯量；为外框架对X_p2、Y_p2、Z_p2轴的转动惯量；为随动框架对X_p3、Y_p3、Z_p3轴的转动惯量。定义为折合到台体轴X_p的转动惯量，为折合到台体轴Y_p的转动惯量；J_xy、J_yx、J_xz、J_yz为框架系统的等效惯量积。其中：

$\begin{array}{c}{J}_{x_p}^{\′}=J_{x_p}+J_{x_{p1}}{\cos }^2{\mathrm{\β}}_{zk}+J_{y_{p1}}{\sin }^2{\mathrm{\β}}_{zk}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{zk}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}\\ +J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\\ +\frac{1}{4}\left(J_{z_{p3}}-J_{y_{p3}}\right)\sin 2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(1\right)$

$\begin{array}{c}{J}_{y_p}^{\′}=J_{y_p}+J_{y_{p1}}{\sin }^2{\mathrm{\β}}_{zk}+J_{y_{p1}}{\cos }^2{\mathrm{\β}}_{zk}+J_{x_{p2}}{\sin }^2{\mathrm{\β}}_{zk}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}\\ +J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\\ -\frac{1}{4}\left(J_{z_{p3}}-J_{y_{p3}}\right)\sin 2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(2\right)$

$\begin{array}{c}{J}_{xy}=\frac{1}{2}\left(J_{x_{p1}}-J_{y_{p1}}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{yk}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\right)\sin 2{\mathrm{\β}}_{zk}\\ +\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p3}}\right){\cos }^2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(3\right)$

$J_{xz}=\frac{1}{2}(J_{z_{p2}}-J_{x_{p2}}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{xk})sin2{\mathrm{\β}}_{yk}{\mathrm{cos\β}}_{zk}---\left(4\right)$

$\begin{array}{c}{J}_{yx}=\frac{1}{2}\left(J_{x_{p1}}-J_{y_{p1}}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{yk}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\right)\sin 2{\mathrm{\β}}_{zk}\\ -\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p1}}\right){\cos }^2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(5\right)$

$J_{yz}=\frac{1}{2}(J_{z_{p2}}-J_{x_{p2}}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{xk})sin2{\mathrm{\β}}_{yk}{\mathrm{sin\β}}_{zk}---\left(6\right)$

设M_zp为台体轴干扰力矩，为台体轴力矩电机反馈力矩；为内框架轴干扰力矩，为内框架轴力矩电机反馈力矩；为外框架轴干扰力矩，为外框架轴力矩电机反馈力矩；为随动框架轴上的外力矩，为随动框架轴力矩电机反馈力矩；则四轴惯性平台系统的各轴端力矩作用到台体三轴的合成力矩如下：

$\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\end{array}\right]\left[\begin{array}{c}{M}_{zp}\\ M_{y_{p1}}\\ M_{x_{p2}}\\ M_{y_{p3}}\end{array}\right]---\left(7\right)$

$\left[\begin{array}{c}{M}_{D_{z3}}\\ M_{D_{y3}}\\ M_{D_{x3}}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\end{array}\right]\left[\begin{array}{c}{M}_{D_{zp}}\\ M_{D_{y1}}\\ M_{D_{x2}}\\ M_{D_{y3}}\end{array}\right]---\left(8\right)$

分别为台体绕x_p、y_p、z_p轴的绝对角速度，可通过正交安装于台体的陀螺仪测量得到；则四轴惯性平台系统的台体动力学方程为

$\left[\begin{array}{ccc}{J}_{x_p}^{\′}& J_{xy}& J_{xz}\\ J_{yx}& J_{y_p}^{\′}& J_{yz}\\ 0& 0& J_{z_p}^{\′}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{x_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{y_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{z_p}\end{array}\right]=\left[\begin{array}{c}{M}_{x3}\\ M_{y3}\\ M_{z3}\end{array}\right]-\left[\begin{array}{c}{M}_{D_{x3}}\\ M_{D_{y3}}\\ M_{D_{z3}}\end{array}\right]---\left(9\right)$

可以看出，在3个陀螺仪角速率信息已知时，有4个控制执行环节伺服回路不完全可控。因此，目前的解决措施是增加随动控制回路，使内框架角β_yk近似为0°，此时有动力学方程

$\begin{array}{c}\left(J_{y_{p2}}+J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{xk}\right){\stackrel{\·\·}{\mathrm{\β}}}_{yk}+\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p3}}\right){\stackrel{\·}{\mathrm{\ω}}}_{z_{p2}}{\mathrm{sin\β}}_{xk}\\ =\left(M_{y_{p3}}-M_{D_{p3}}\right){\mathrm{cos\β}}_{xk}+M_{z_{p3}}{\mathrm{sin\β}}_{xk}\end{array}---\left(10\right)$

结合台体三轴动力学方程后，四轴惯性平台系统的动力学方程为：

$\left[\begin{array}{cccc}{J}_{z_p}^{\′}& 0& 0& 0\\ J_{yz}& J_{y_p}^{\′}& J_{yx}& 0\\ J_{xz}& 0& J_{x_p}^{\′}& 0\\ 0& 0& 0& J_{y_{p2}}^{\′}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{z_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{y_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{x_p}\\ {\stackrel{\·\·}{\mathrm{\β}}}_{yk}\end{array}\right]=\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\\ M_{y2}\end{array}\right]-\left[\begin{array}{c}{M}_{D_{z3}}\\ M_{D_{y3}}\\ M_{D_{x3}}\\ M_{D_{y2}}\end{array}\right]---\left(11\right)$

其中，

$\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\\ M_{y3}\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\\ 0& 0& 0& {\mathrm{cos\β}}_{xk}\end{array}\right]\left[\begin{array}{c}{M}_{zp}\\ M_{y_{p1}}\\ M_{x_{p2}}\\ M_{y_{p3}}\end{array}\right]---\left(12\right)$

$J_{y_{p2}}^{\′}=J_{y_{p2}}+J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{xk}$

此时，空间解耦矩阵在β_yk近似为0时，可简化为

$T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& -{\mathrm{sin\β}}_{zk}& 0\\ 0& {\mathrm{sec\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sec\β}}_{yk}{\mathrm{cos\β}}_{zk}& -{\mathrm{tan\β}}_{xk}{\mathrm{tan\β}}_{yk}\\ 0& 0& 0& {\mathrm{sec\β}}_{xk}\end{array}\right]\stackrel{{\mathrm{\β}}_{yk}=0}{\≈}\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& -{\mathrm{sin\β}}_{zk}& 0\\ 0& {\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{zk}& 0\\ 0& 0& 0& {\mathrm{sec\β}}_{xk}\end{array}\right]---\left(13\right)$

如图2所示的现有解耦伺服回路原理框图，现有技术在在原三个稳定回路的坐标分解器基础上，增加正割分解器secβ_xk。即把作为随动回路的一个环节。但也看出，在β_xk趋于±90°时，存在奇异值，secβ_xk趋于无穷大。

发明内容

本发明的目的在于克服现有技术的不足，提供一种四轴惯性稳定平台系统的伺服回路解耦方法，该方法可以在平台系统相对转动角度为任意值的情况下，实现无奇异值的伺服回路解耦计算，从而提高了载体无轨迹约束条件下的全姿态适应能力。

本发明的上述目的通过以下技术方案实现：

一种四轴惯性稳定平台系统的伺服回路解耦方法，基于四轴惯性稳定平台系统实现，所述稳定平台系统包括基座、随动框架、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、随动框架坐标系X_p3Y_p3Z_p3、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p；所述五个坐标系的原点重合，并且：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架的本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，随动框架本体坐标系的X_p3轴与外框架本体坐标系的X_p2轴重合，基座本体坐标系的X₁轴与随动框架本体坐标系的Y轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕随动框架本体坐标系的Y_p3轴转动，随动框架绕外框架本体坐标系的X_p2轴转动，外框架绕内框架本体坐标系的Y_p1轴转动，内框架绕台体本体坐标系的Z_p轴转动；

所述四轴惯性平台系统伺服回路解耦方法实现步骤如下：

(1)、根据台体上安装的陀螺仪输出的角速度，得到台体在X_p轴、Y_p轴和Z_p轴上的角速度分量

(2)、测量得到四轴惯性稳定平台系统内部相对转动的角度和角速度，包括：随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk，外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度内框架绕台体本体坐标系的Z_p轴转动的角度β_zk和角速度

(3)、计算台体、内框架、外框架和随动框架的转动角速度，具体计算公式如下：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p},$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

其中，ω_z为台体Z_p轴的合成转动角速度；ω_y为内框架Y_p1轴的合成转动角速度；ω_x为外框架X_p2轴的合成转动角速度；ω_yk′为随动框架Y_p3轴的合成转动角速度。

上述的四轴惯性稳定平台系统的伺服回路解耦方法，在步骤(2)中，通过如下方法测量得到四轴惯性稳定平台系统内部相对转动角度和角速度：

在外框架的X_p2轴上安装角度传感器，测量得到随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk；在内框架的Y_p1轴上安装角度传感器，测量得到外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk和角速度

上述的四轴惯性稳定平台系统的伺服回路解耦方法，在步骤(2)中，转动角度β_xk、β_yk、β_zk的取值范围为0～360°。

本发明与现有技术相比具有以下优点：

(1)、本发明给出的解耦计算公式，可以在系统内部相对转动角0～360°范围内实现无奇异值计算，克服了现有技术在外框架角β_xk＝±90°、内框架角β_yk＝±90°时的奇异值问题；相比现有的解耦方法更准确、适用性更广；

(2)、本发明在解耦计算中，在原相对角速度基础上进行正余弦计算，不存在增益放大的问题，避免了现有解耦方法中增益趋于无穷大的问题。

附图说明

图1为四轴惯性稳定平台系统中四个本体坐标系之间的关系示意图；

图2为现有技术中采用的解耦方案中动调陀螺四轴惯性稳定平台伺服回路原理框图；

图3为本发明的四轴惯性平台系统伺服回路解耦方法；

图4为本发明采用的解耦方案中动调陀螺四轴惯性稳定平台伺服回路原理框图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

本发明提供的一种四轴惯性稳定平台系统的伺服回路解耦方法，基于四轴惯性稳定平台系统实现。该四轴稳定平台系统包括基座、随动框架、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、随动框架坐标系X_p3Y_p3Z_p3、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p。

如图1所示的五个坐标系的关系示意图，以上所述的五个坐标系的原点重合，并且存在如下相对约束关系：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架的本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，随动框架本体坐标系的X_p3轴与外框架本体坐标系的X_p2轴重合，基座本体坐标系的X₁轴与随动框架本体坐标系的Y轴重合。其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时：基座绕随动框架本体坐标系的Y_p3轴转动且转动角度为β_yk′；随动框架绕外框架本体坐标系的X_p2轴转动且转动角度为β_xk；外框架绕内框架本体坐标系的Y_p1轴转动且转动角度为β_yk，内框架绕台体本体坐标系的Z_p轴转动且转动角度为β_zk。

如图3所示的处理流程图，本发明的四轴惯性平台系统伺服回路解耦方法实现步骤如下：

(1)、根据台体上安装的陀螺仪输出的角速度，得到台体在X_p轴、Y_p轴和Z_p轴上的角速度分量

(2)、通过如下方法测量得到四轴惯性稳定平台系统内部相对转动角度和角速度：

在外框架的X_p2轴上安装角度传感器，测量得到随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk；在内框架的Y_p1轴上安装角度传感器，测量得到外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk和角速度其中，以上测量得到的相对转动角度β_xk、β_yk、β_zk的取值范围为0～360°，即该方法适用于全姿态计算。

(3)、计算台体、内框架、外框架和随动框架的转动角速度，具体计算公式如下：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p},$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

其中，ω_z为台体Z_p轴的合成转动角速度；ω_y为内框架Y_p1轴的合成转动角速度；ω_x为外框架X_p2轴的合成转动角速度；ω_yk′为随动框架Y_p3轴的合成转动角速度。

根据本发明提供的四轴惯性平台系统伺服回路解耦方法，在工程应用中的伺服回路原理框图如图4所示。

实施例1：

在本实施例中，利用本发明的计算公式进行解耦计算，其中设定条件如下：基座绕外框架坐标系X_p2轴转动的角度β_xk＝0；外框架绕内框架坐标系Y_p1轴转动的角度β_yk＝0；内框架绕台体坐标系Z_p轴转动的角度β_zk＝0；即三个转动轴之间相互垂直。

根据本发明提供计算公式可得：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{x_p};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\stackrel{\·}{\mathrm{\β}}}_{yk};$

从以上计算结果可以看出，台体三轴控制器输入量分别与各自陀螺仪的测量值一致，随动框架控制量输入与内框架角速度有关。

实施例2：

在本实施例中，利用本发明的计算公式进行解耦计算，其中设定条件如下：基座绕外框架坐标系X_p2轴转动的角度β_xk＝90°；外框架绕内框架坐标系Y_p1轴转动的角度β_yk＝0；内框架绕台体坐标系Z_p轴转动的角度β_zk＝0。

根据本发明提供计算公式可得：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{x_p};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\stackrel{\·}{\mathrm{\β}}}_{zk};$

从以上计算结果可以看出，台体三轴控制器输入量分别与各自陀螺仪的测量值一致，随动框架控制量输入与台体角速度有关。

实施例3：

在本实施例中，利用本发明的计算公式进行解耦计算，其中设定条件如下：基座绕外框架坐标系X_p2轴转动的角度β_xk＝90°；外框架绕内框架坐标系Y_p1轴转动的角度β_yk＝90°；内框架绕台体坐标系Z_p轴转动的角度β_zk＝0。

根据本发明提供计算公式可得：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x=-{\stackrel{\·}{\mathrm{\β}}}_{zk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{x_p};$

从以上计算结果可以看出，除台体Y和Z轴控制量分别与各自陀螺仪的测量值一致外，随动框架控制器输入量与X陀螺仪有关，外框架输入量与台体轴角速率有关。

上述三个实施例可以验证本发明的解耦方法正确。

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

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

Claims (3)

1.一种四轴惯性稳定平台系统的伺服回路解耦方法，其特征在于：基于四轴惯性稳定平台系统实现，所述稳定平台系统包括基座、随动框架、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、随动框架坐标系X_p3Y_p3Z_p3、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p；所述五个坐标系的原点重合，并且：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架的本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，随动框架本体坐标系的X_p3轴与外框架本体坐标系的X_p2轴重合，基座本体坐标系的X₁轴与随动框架本体坐标系的Y轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕随动框架本体坐标系的Y_p3轴转动，随动框架绕外框架本体坐标系的X_p2轴转动，外框架绕内框架本体坐标系的Y_p1轴转动，内框架绕台体本体坐标系的Z_p轴转动；

所述四轴惯性平台系统伺服回路解耦方法实现步骤如下：

(1)、根据台体上安装的陀螺仪输出的角速度，得到台体在X_p轴、Y_p轴和Z_p轴上的角速度分量

(2)、测量得到四轴惯性稳定平台系统内部相对转动的角度和角速度，包括：随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk，外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度内框架绕台体本体坐标系的Z_p轴转动的角度β_zk和角速度

(3)、计算台体、内框架、外框架和随动框架的转动角速度，具体计算公式如下：

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

其中，ω_z为台体Z_p轴的合成转动角速度；ω_y为内框架Y_p1轴的合成转动角速度；ω_x为外框架X_p2轴的合成转动角速度；ω_yk′为随动框架Y_p3轴的合成转动角速度。

2.根据权利要求1所述的一种四轴惯性稳定平台系统的伺服回路解耦方法，其特征在于：在步骤(2)中，通过如下方法测量得到四轴惯性稳定平台系统内部相对转动角度和角速度：

在外框架的X_p2轴上安装角度传感器，测量得到随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk；在内框架的Y_p1轴上安装角度传感器，测量得到外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk和角速度

3.根据权利要求1或2所述的一种四轴惯性稳定平台系统的伺服回路解耦方法，其特征在于：在步骤(2)中，转动角度β_xk、β_yk、β_zk的取值范围为0～360°。