CN109506649B - 一种四轴惯性稳定平台系统内框架锁零方法及系统 - Google Patents

Abstract

本发明提供了一种四轴惯性稳定平台系统内框架锁零方法及系统，以正交安装于平台台体的3个陀螺仪输出角速率

和内框架角速度等4个变量作为解耦环节的输入信息，通过信息融合后输出4个分别作用到台体轴、内环轴、外环轴和随动轴的轴端力矩电机。本发明首次给出了四轴平台在外框架角为90°时的内框架锁零方法，以及各框架角在稳定时的值，实现了平台台体相对于惯性空间的稳定。该方法实现了外框架角为90°时内框架角始终保持于零位的要求，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

Description

一种四轴惯性稳定平台系统内框架锁零方法及系统

技术领域

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

背景技术

由于三轴惯性平台系统存在“框架锁定”现象，难以满足载体大机动运动的要求，引起“框架锁定”的主要原因是内框架角工作于非零状态，特别是内框架角为90°时引起三个电机的力矩处于一个平面，从而导致平台台体中垂直于该平面的矢量方向缺少克服载体运动的能力，会引起平台台体相对惯性空间不能稳定。因此，产生了四轴惯性稳定平台系统。

四轴惯性平台系统相对三轴惯性平台系统，在台体、内框架和外框架的基础上增加了随动框架，随动框架处于平台外框架和基座之间。随动回路信号来自于内框架角，采用正割分解器进行增益补偿，其核心是使内框架角工作于0°。下面举例介绍四轴惯性稳定平台随动回路工作的原理。

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

在β_zk＝0、β_yk＝0、β_xk＝0、β_yk′＝0时，包含台体、内框架、外框架、随动框架和基座的平台结构如图2所示。此时，平台框架可以隔离基座的角运动，台体相对惯性空间稳定，且可保证内框架角β_yk＝0。在随动回路不工作时，如果基座OX₁轴上存在角速度ω_x1，基座会带动随动框架、外框架绕内框架轴转动，使得转动角度β_yk不为0。而在随动回路工作时，以β_yk作为控制器的输入量，作用到随动轴电机以克服角速度ω_x1，实现了内框架角β_yk＝0。

但有一个特殊情况，不能保证内框架角β_yk＝0。当β_xk＝90°时，如图3所示，随动回路将不具备使内框架角β_yk＝0的功能，此时，如果基座带动随动框架和外框架一起绕内框架轴转动时，则内框架角将不为0。进一步，如果内框架上面有限位档钉，则会带动台体转动，从而引起台体相对惯性空间转动。

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

为此，需要研究一种使四轴惯性稳定平台系统内框架角度β_yk始终为0的解耦方法。

发明内容

本发明的目的在于克服现有技术的不足，提供一种四轴惯性稳定平台系统内框架角锁零方法及系统，实现了外框架角为90°时内框架角始终保持于零位的要求，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

本发明的上述目的通过以下技术方案实现：一种四轴惯性稳定平台系统内框架锁零方法，基于四轴惯性稳定平台系统实现，所述稳定平台系统包括基座、随动框架、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系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_p3轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕随动框架本体坐标系的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)、计算台体、内框架、外框架和随动框架的转动角速度，具体解耦计算公式如下：

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

(4)、采用上述解耦计算公式，可得平台四个框架角角速度确定方程；(平台四个框架角，包括：基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′，架绕外框架本体坐标系的X_p2轴转动的角度β_xk，外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk和角速度

内框架绕台体本体坐标系的Z_p轴转动的角度β_zk)

(5)、根据平台四个框架角角速度确定方程、四个框架角度和台体上安装的陀螺仪测量的角速度，对内框架锁零条件进行判断，如下：

1)、在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，不需要对内框架锁零使使平台台体相对惯性空间稳定；

2)、在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，不需要对内框架锁零使平台台体相对惯性空间稳定；

3)、在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，需要对内框架锁零才能使平台台体相对惯性空间稳定；

(6)、在需要对内框架锁零才能使平台台体相对惯性空间稳定时，由随动框架带动外框架和内框架一起相对台体快速发生转动，转动前后内框架角β_yk恒为零，保证了平台台体相对惯性空间仍然稳定。

在步骤(6)中，随动框架带动外框架和内框架一起相对台体快速发生转动后的稳定位置的框架角度值计算过程为：

(1)、测量得到β_xk、β_yk和β_zk的初始值分别为β_xk0、β_yk0和β_zk0；

(2)、设基座本体坐标系下平台基座的角速度为

当

中有一个为非零时，基座绕随动框架本体坐标系的Y_p3轴转动的角速度

随动框架绕外框架本体坐标系的X_p2轴转动的角速度

外框架绕内框架本体坐标系的Y_p1轴转动的角速度

内框架绕台体本体坐标系的Z_p轴转动的角速度

的表达式分别为

其中，

(3)、确定基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′、随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk、外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk、内框架绕台体本体坐标系的Z_p轴转动的角度β_zk的稳态值，即实现对内框架锁零以保证平台台体相对惯性空间仍然稳定的角度值，分为以下四种情况：

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减。

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°。

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0。

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

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

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

在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk。

在步骤(2)中，转动角度β_yk′、β_xk、β_yk、β_zk的取值范围为-180°～+180°。

传感器采用光电编码器或正余弦旋转编码器。

步骤(3)中计算台体、内框架、外框架和随动框架的合成转动角速度采用数字计算机实现。

四轴惯性稳定平台系统的基座与载体固连。

随动框架上安装的力矩电机输出力矩大于1N·m。

台体上安装的陀螺仪为双自由度陀螺，数量需要至少两个，两个正交安装。

台体上安装的陀螺仪为单自由度陀螺，数量需要至少三个，两两正交安装。

本发明一种四轴惯性稳定平台系统内框架锁零系统，包括：确定模块、测量模块、角速度确定模块、方程确定模块、判断模块和稳定模块；

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

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

内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

角速度确定模块采用解耦计算公式计算台体、内框架、外框架和随动框架的转动角速度；

方程确定模块采用角速度确定模块中的解耦计算公式，得到平台四个框架角角速度确定方程；

判断模块根据平台四个框架角角速度确定方程、四个框架角度和台体上安装的陀螺仪测量的角速度，对内框架锁零条件进行判断，如下：

1)在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，不需要对内框架锁零使使平台台体相对惯性空间稳定；

2)在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，不需要对内框架锁零使平台台体相对惯性空间稳定；

3)在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，需要对内框架锁零才能使平台台体相对惯性空间稳定；

稳定模块在需要对内框架锁零才能使平台台体相对惯性空间稳定时，由随动框架带动外框架和内框架一起相对台体快速发生转动，转动前后内框架角β_yk恒为零，保证平台台体相对惯性空间仍然稳定。

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

(1)本发明给出的一种四轴惯性稳定平台系统内框架锁零方法及系统，完全覆盖了4个姿态角在任意象限的情况，克服了原有技术在外框架角β_xk＝±90°时的内框架非零的问题；

(2)本发明给出了一种四轴惯性稳定平台系统内框架锁零方法及系统，虽然在计算环节中存在secβ_xk，但给出了在该奇异点处的框架角稳态值，确保了系统仍然稳定而不发散。

(3)本发明实现了外框架角为90°时内框架角始终保持于零位的要求，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

附图说明

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

图2为四个框架角度为零时的四轴平台结构示意图；

图3为外框架角β_xk＝90°时的四轴平台结构示意图；

图4为本发明采用的四轴惯性稳定平台锁零前的结构示意图；

图5为本发明采用的四轴惯性稳定平台锁零后的结构示意图；

图6为本发明实现内框架锁零的四个框架角仿真结果；

图7为本发明实现内框架锁零的台体OY_p和OX_p轴的角速度仿真结果；

图8为本发明的流程图。

具体实施方式

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

本发明提供的一种四轴惯性稳定平台系统内框架锁零方法及系统，基于四轴惯性稳定平台系统实现，所述稳定平台系统包括基座、随动框架、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系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_p3轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕随动框架本体坐标系的Y_p3轴转动，随动框架绕外框架本体坐标系的X_p2轴转动，外框架绕内框架本体坐标系的Y_p1轴转动，内框架绕台体本体坐标系的Z_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。

所述四轴惯性稳定平台系统内框架锁零方法实现步骤如下：

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

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

内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

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

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

(4)、在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，使平台台体相对惯性空间稳定；

(5)、在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，平台台体相对惯性空间稳定；

(6)、在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，随动框架带动外框架和内框架一起相对台体快速发生转动，内框架角β_yk＝0°，保证了平台台体相对惯性空间仍然稳定。

采用上述计算公式，可得框架的角速度表达式分别为

其中，

从而可根据以下四种情况，得到四个框架角的稳态值。

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减。

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°。

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0。

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

为形象说明本发明提供的四轴惯性平台系统内框架锁零方法，示意图如图4和图5所示。图4是在图3的基础上，当ω_z1≠0的瞬时，随动框架带动外框架、内框架一起绕基座OX₁和台体OZ_p快速转动，到达图5的位置。此时，基座带动随动框架一起绕OZ₁转动，在转动过程中平台台体相对惯性空间保持稳定。

优选实施例为：

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

时，sinα＝-1，cosα＝0，因此，α＝-90°；由cos(β_yk′+α)＝+1，求得β_yk′＝-α＝90°，如图6所示，β_yk′快速稳定在+90°，β_zk也快速跟随到β_zk＝α＝-90°，

的速度逐渐减小；在此过程中，内框架角β_yk近似为0。平台台体OY_p和OX_p轴的角速度仿真结果如图7所示，可以看出，在内框架锁零过程中台体角速度的值非常小(10^-37～10^-36°/s量级)，可以忽略为零。图6左上图纵坐标为Bzk表示β_zk，右上图纵坐标为Byk表示β_yk，左下图纵坐标为Bxk表示β_xk，右下图中Bykp表示β_yk′，.Time表示时间。图7中，wxp表示

wyp表示

Time表示时间。

上述实施例可以验证本发明的内框架锁零方法正确，图8为实现本发明的流程图。

本发明一种四轴惯性稳定平台系统内框架锁零系统，包括：确定模块、测量模块、角速度确定模块、方程确定模块、判断模块和稳定模块；

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

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

内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

角速度确定模块采用解耦计算公式计算台体、内框架、外框架和随动框架的转动角速度；

方程确定模块采用角速度确定模块中的解耦计算公式，得到平台四个框架角角速度确定方程；

判断模块根据平台四个框架角角速度确定方程、四个框架角度和台体上安装的陀螺仪测量的角速度，对内框架锁零条件进行判断，如下：

1)在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，不需要对内框架锁零使使平台台体相对惯性空间稳定；

2)在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，不需要对内框架锁零使平台台体相对惯性空间稳定；

3)在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，需要对内框架锁零才能使平台台体相对惯性空间稳定；

稳定模块在需要对内框架锁零才能使平台台体相对惯性空间稳定时，由随动框架带动外框架和内框架一起相对台体快速发生转动，转动前后内框架角β_yk恒为零，保证平台台体相对惯性空间仍然稳定。

稳定模块中，随动框架带动外框架和内框架一起相对台体快速发生转动后的稳定位置的框架角度值计算过程为：

(1)、测量得到β_xk、β_yk和β_zk的初始值分别为β_xk0、β_yk0和β_zk0；

(2)、设基座本体坐标系下平台基座的角速度为

当

中有一个为非零时，基座绕随动框架本体坐标系的Y_p3轴转动的角速度

随动框架绕外框架本体坐标系的X_p2轴转动的角速度

外框架绕内框架本体坐标系的Y_p1轴转动的角速度

内框架绕台体本体坐标系的Z_p轴转动的角速度

的表达式分别为

其中，

(3)、确定基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′、随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk、外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk、内框架绕台体本体坐标系的Z_p轴转动的角度β_zk的稳态值，即实现对内框架锁零以保证平台台体相对惯性空间仍然稳定的角度值，分为以下四种情况：

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减。

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°。

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0。

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

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

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

在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk。

测量模块中，转动角度β_yk′、β_xk、β_yk、β_zk的取值范围为-180°～+180°。

传感器采用光电编码器或正余弦旋转编码器。

角速度确定模块中计算台体、内框架、外框架和随动框架的合成转动角速度采用数字计算机实现。

四轴惯性稳定平台系统的基座与载体固连。

随动框架上安装的力矩电机输出力矩大于1N·m。

台体上安装的陀螺仪为双自由度陀螺，数量需要至少两个，两个正交安装。

台体上安装的陀螺仪为单自由度陀螺，数量需要至少三个，两两正交安装。

本发明给出的一种四轴惯性稳定平台系统内框架锁零方法及系统，完全覆盖了4个姿态角在任意象限的情况，克服了原有技术在外框架角β_xk＝±90°时的内框架非零的问题；本发明虽然在计算环节中存在secβ_xk，但给出了在该奇异点处的框架角稳态值，确保了系统仍然稳定而不发散。本发明实现了外框架角为90°时内框架角始终保持于零位的要求，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

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

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

Claims (9)

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_p3轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕随动框架本体坐标系的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)采用解耦计算公式计算台体、内框架、外框架和随动框架的转动角速度，具体解耦计算公式如下：

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

(4)采用步骤(3)的解耦计算公式，得到平台四个框架角角速度确定方程，如下：

其中，

(5)根据平台四个框架角角速度确定方程、四个框架角度和台体上安装的陀螺仪测量的角速度，对内框架锁零条件进行判断，如下：

1)在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，不需要对内框架锁零使使平台台体相对惯性空间稳定；

2)在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，不需要对内框架锁零使平台台体相对惯性空间稳定；

3)在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，需要对内框架锁零才能使平台台体相对惯性空间稳定；

(6)在需要对内框架锁零才能使平台台体相对惯性空间稳定时，由随动框架带动外框架和内框架一起相对台体快速发生转动，转动前后内框架角β_yk恒为零，保证平台台体相对惯性空间仍然稳定；

由随动框架带动外框架和内框架一起相对台体快速发生转动后的稳定位置的框架角度值，计算过程为：

1)测量得到β_xk、β_yk和β_zk的初始值分别为β_xk0、β_yk0和β_zk0；

2)设基座本体坐标系下平台基座的角速度为

当

中有一个为非零时，基座绕随动框架本体坐标系的Y_p3轴转动的角速度

随动框架绕外框架本体坐标系的X_p2轴转动的角速度

外框架绕内框架本体坐标系的Y_p1轴转动的角速度

内框架绕台体本体坐标系的Z_p轴转动的角速度

的表达式分别为

其中，

3)确定基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′、随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk、外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk、内框架绕台体本体坐标系的Z_p轴转动的角度β_zk的稳态值，即实现对内框架锁零以保证平台台体相对惯性空间仍然稳定的角度值，分为以下四种情况：

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减；

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°；

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0；

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

2.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：在步骤(6)中，随动框架带动外框架和内框架一起相对台体快速发生转动后的稳定位置的框架角度值，计算过程为：

(1)测量得到β_xk、β_yk和β_zk的初始值分别为β_xk0、β_yk0和β_zk0；

(2)设基座本体坐标系下平台基座的角速度为

当

中有一个为非零时，基座绕随动框架本体坐标系的Y_p3轴转动的角速度

随动框架绕外框架本体坐标系的X_p2轴转动的角速度

外框架绕内框架本体坐标系的Y_p1轴转动的角速度

内框架绕台体本体坐标系的Z_p轴转动的角速度

的表达式分别为

其中，

(3)确定基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′、随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk、外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk、内框架绕台体本体坐标系的Z_p轴转动的角度β_zk的稳态值，即实现对内框架锁零以保证平台台体相对惯性空间仍然稳定的角度值，分为以下四种情况：

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减；

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°；

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0；

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

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

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

在台体Z_p轴上安装传感器测量内框架绕台体本体坐标系的Zp轴转动的角度β_zk。

4.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：在步骤(2)中，转动角度β_yk′、β_xk、β_yk、β_zk的取值范围为-180°～+180°。

5.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：传感器采用光电编码器或正余弦旋转编码器。

6.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：步骤(3)中计算台体、内框架、外框架和随动框架的合成转动角速度采用数字计算机实现。

7.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：四轴惯性稳定平台系统的基座与载体固连。

8.根据权利要求1所述的一种四轴惯性稳定平台系统内框架锁零方法，其特征在于：随动框架上安装的力矩电机输出力矩大于1N·m。

9.一种四轴惯性稳定平台系统内框架锁零系统，其特征在于包括：确定模块、测量模块、角速度确定模块、方程确定模块、判断模块和稳定模块；

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

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

内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

角速度确定模块采用解耦计算公式计算台体、内框架、外框架和随动框架的转动角速度，具体解耦计算公式如下：

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

方程确定模块采用角速度确定模块中的解耦计算公式，得到平台四个框架角角速度确定方程，如下：

其中，

判断模块根据平台四个框架角角速度确定方程、四个框架角度和台体上安装的陀螺仪测量的角速度，对内框架锁零条件进行判断，如下：

1)在β_xk≠90°且β_xk≠-90°时，平台内框架角速度为0，不需要对内框架锁零使使平台台体相对惯性空间稳定；

2)在β_xk＝90°或β_xk＝-90°，以及基座角速度

时，不需要对内框架锁零使平台台体相对惯性空间稳定；

3)在β_xk＝90°或β_xk＝-90°，当

中有一个为非零时，需要对内框架锁零才能使平台台体相对惯性空间稳定；

稳定模块在需要对内框架锁零才能使平台台体相对惯性空间稳定时，由随动框架带动外框架和内框架一起相对台体快速发生转动，转动前后内框架角β_yk恒为零，保证平台台体相对惯性空间仍然稳定；

由随动框架带动外框架和内框架一起相对台体快速发生转动后的稳定位置的框架角度值，计算过程为：

1)测量得到β_xk、β_yk和β_zk的初始值分别为β_xk0、β_yk0和β_zk0；

2)设基座本体坐标系下平台基座的角速度为

当

中有一个为非零时，基座绕随动框架本体坐标系的Y_p3轴转动的角速度

随动框架绕外框架本体坐标系的X_p2轴转动的角速度

外框架绕内框架本体坐标系的Y_p1轴转动的角速度

内框架绕台体本体坐标系的Z_p轴转动的角速度

的表达式分别为

其中，

3)确定基座绕随动框架本体坐标系的Y_p3轴转动的角度β_yk′、随动框架绕外框架本体坐标系的X_p2轴转动的角度β_xk、外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk、内框架绕台体本体坐标系的Z_p轴转动的角度β_zk的稳态值，即实现对内框架锁零以保证平台台体相对惯性空间仍然稳定的角度值，分为以下四种情况：

(a)β_xk<90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_yk′0；此时，由于

所以β_zk的值会增大，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0；由于

所以β_xk递减；

(b)β_xk>90°且β_xk→90°时，sinβ_xk＝1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_yk′0；此时，由于

所以β_zk的值会减小，稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_yk′0-180°；

(c)β_xk<-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk>0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝+1，即β_yk′＝-α，其中，β_yk′的变化量为Δβ_yk′＝-α-β_xk0；此时，由于

所以β_zk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0+α+β_xk0；

(d)β_xk>-90°且β_xk→-90°时，sinβ_xk＝-1，tanβ_xk<0；β_yk′的初始值为β_yk′0，为保证系统稳定，则有cos(β_yk′+α)＝-1，即β_yk′＝180°-α，其中，β_yk′的变化量为Δβ_yk′＝180°-α-β_xk0；此时，由于

所以β_zk的值会减小；β_zk的稳态值为β_zk＝β_zk0-Δβ_yk′＝β_zk0-180°+α+β_xk0。

Images