CN109540134B - 一种三轴稳定平台***框架自解锁方法及*** - Google Patents

Abstract

本发明公开了一种三轴稳定平台***框架自解锁方法及***，以正交安装于平台台体的3个陀螺仪输出角速率

等3个变量作为解耦环节的输入信息，通过信息融合后输出3个分别作用到台体轴、内环轴和外环轴的轴端力矩电机。本发明首次给出了三轴平台在“框架锁定”时的解锁方法，以及解锁后的框架角，实现了平台台体相对于惯性空间的稳定。本发明给出了在奇异点处的框架角稳态值，确保了***仍然稳定而不发散，该方法实现了三轴平台在“框架锁定”时的快速解耦和框架状态切换，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

Description

一种三轴稳定平台***框架自解锁方法及***

技术领域

本发明涉及一种三轴稳定平台***框架自解锁方法及***，尤其涉及三轴平台***的伺服回路多变量解耦方法及***，主要用于实现全姿态高精度导航的航空、航天领域。

背景技术

三轴稳定平台已广泛地用于机动姿态有限的载体上，即用于在飞行中不会同时绕两个轴出现大姿态角的载体中。但有时由于运载火箭和弹道式导弹要做机动变轨飞行；特别是战术导弹、卫星以及许多军用飞机需要在全姿态、大机动状态下工作。在这样条件下，要求平台台体仍能保持稳定。

由于三轴稳定平台***存在“框架锁定”现象，即内框架和外框架在一个平面内，此时，台体轴、内框架轴和外框架轴也同时在一个平面内，从而使平台失去一个自由度。另一方面，原来用于控制外框架轴的陀螺仪不能感受到外框架轴的转动，从而失去了对外框架轴的控制作用。

下面举例介绍“框架锁定”发生时的具体情况。

首先，三轴惯性平台***的坐标系定义如图1所示，描述了三轴平台各框架坐标系之间关系的示意图。在图1中，设β_zk为内框架相对台体的相对角度，β_yk为外框架相对内框架的相对角度，β_xk为为基座(箭体)相对外框架的相对角度。

在β_zk＝0、β_yk＝0、β_xk＝0时，包含台体、内框架、外框架和基座的平台结构如图2所示。此时，平台框架可以隔离基座的脚运动，台体相对惯性空间稳定。在基座OY₁轴上存在角速度ω_y1时，基座带动外框架绕外框架轴转动，转动角度为β_yk。当β_yk＝90°时，外框架轴、内框架轴和台体轴三者在一个平面内，三轴平台“框架锁定”，如图3所示。此时，由于三轴平台在垂直于该平面的OZ₁轴方向不能隔离角运动，当ω_z1≠0时，则平台台体ω_xp≠0，意味着平台台体相对惯性空间发生转动，不能隔离载体的角运动。

为了消除在内框架角为90°时稳定回路的失控，避免框架***得锁定，目前，三轴稳定平台***主要的解决措施是在内框架增加挡钉，以限制内框架角的运动范围。比如，通过增加挡钉使内框架角工作在±20°或±40°的范围内。现有措施只能满足机动姿态有限的载体上，难以满足载体大机动运动的要求。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种三轴惯性平台***框架自解锁方法及***，实现了三轴平台在“框架锁定”时的快速解耦和框架状态切换，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

本发明的上述目的通过以下技术方案实现：一种三轴惯性稳定平台***框架自解锁方法，基于三轴惯性稳定平台***实现，所述稳定平台***包括基座、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p；四个坐标系的原点重合，并且：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，基座本体坐标系的X₁轴与外框架本体坐标系的X_p2轴重合；其中，基座与载体固连，在所述稳定平台***在载体带动下发生内部相对转动时，基座绕外框架本体坐标系的X_p2轴转动，外框架绕内框架本体坐标系的Y_p1轴转动，内框架绕台体本体坐标系的Z_p轴转动；

所述三轴惯性稳定平台***框架自解锁方法实现步骤如下：

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

(2)测量得到三轴惯性稳定平台***内部相对转动的角度，包括：基座绕外框架本体坐标系的X_p2轴转动的角度β_xk，外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk，内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

(3)采用解耦计算公式计算台体、内框架和外框架的合成转动角速度；

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

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

1)在β_yk≠90°且β_yk≠-90°时，平台内框架和外框架不在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

时，平台内框架和外框架在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

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

(6)在需要框架自解锁才能使平台台体相对惯性空间稳定时，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定。

步骤(3)采用解耦计算公式计算台体、内框架和外框架的合成转动角速度，具体解耦计算公式如下：

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

在步骤(6)中，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定的角度值计算过程为：

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

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

当

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

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

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

的表达式分别为

其中，

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

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

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

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

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

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

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

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

所以β_yk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_xk＝β_zk0+α+β_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)在β_yk≠90°且β_yk≠-90°时，平台内框架和外框架不在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

时，平台内框架和外框架在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

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

自解锁模块在需要框架自解锁才能使平台台体相对惯性空间稳定时，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定。

本发明与现有技术相比的优点如下：

(1)本发明给出的一种三轴惯性平台***框架自解锁方法，完全覆盖了3个姿态角在任意象限的情况，克服了原有技术在内框架角β_yk＝±90°时的“框架锁定”问题；

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

(3)本发明实现了三轴平台在“框架锁定”时的快速解耦和框架状态切换，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

附图说明

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

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

图3为“框架锁定”时的三轴平台结构示意图；

图4为本发明采用的自解锁方案中三轴惯性稳定平台解锁前的结构示意图；

图5为本发明采用的自解锁方案中三轴惯性稳定平台解锁后的结构示意图；

图6为本发明实现自解锁的三个框架角仿真结果；

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

图8为本发明的流程图。

具体实施方式

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

本发明公开了一种三轴稳定平台***框架自解锁方法，以正交安装于平台台体的3个陀螺仪输出角速率

等3个变量作为解耦环节的输入信息，通过信息融合后输出3个分别作用到台体轴、内环轴和外环轴的轴端力矩电机。本发明首次给出了三轴平台在“框架锁定”时的解锁方法，以及解锁后的框架角，实现了平台台体相对于惯性空间的稳定。本发明给出了在奇异点处的框架角稳态值，确保了***仍然稳定而不发散，该方法实现了三轴平台在“框架锁定”时的快速解耦和框架状态切换，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

本发明基于三轴惯性稳定平台***实现，所述稳定平台***包括基座、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p；所述四个坐标系的原点重合，并且：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架的本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，基座本体坐标系的X₁轴与外框架本体坐标系的X_p2轴重合；其中，基座与载体固连，在所述稳定平台***在载体带动下发生内部相对转动时，基座绕外框架本体坐标系的X_p2轴转动，外框架绕内框架本体坐标系的Y_p1轴转动，内框架绕台体本体坐标系的Z_p轴转动；

计算台体、内框架和外框架的合成转动角速度，具体计算公式如下：

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

采用上述解耦计算公式，可得平台台体本体坐标系X_pY_pZ_p三个方向上的角速度为

内框架本体坐标系X_p1Y_p1Z_p1三个方向上的角速度为

外框架本体坐标系X_p2Y_p2Z_p2三个方向上的角速度为

三个框架角角速度为

设

则有

采用李雅普诺夫定律判断稳定性。方程

由sin(β_xk+α)＝0，有两个解β_xk＝-α和β_xk＝π-α。

对上式进行线性化后的微分方程为

在稳定性分析时，需分为四种情况：

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

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

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

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

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

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

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

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

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

优选的实施例为：

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

时，sinα＝1，cosα＝0，因此，α＝90°；由cos(β_xk+α)＝-1，求得β_xk＝180°-α＝90°，如图6所示，β_xk快速稳定在+90°，β_zk也快速跟随到+90°，β_yk以-1°/s的速度逐渐减小；平台台体OY_p和OX_p轴的角速度仿真结果如图7所示，可以看出，在自解锁过程中台体角速度的值非常小(10^-10～10^-9°/s量级)，可以忽略为零。图6上图纵坐标为Bzk表示β_zk，中图纵坐标为Byk表示β_yk，下图纵坐标为Bxk表示β_xk，Time表示时间，0点表示。图7中，wxp表示

wyp表示

wzp表示

Time表示时间。

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

本发明的一种三轴惯性稳定平台***框架自解锁***，包括：确定模块、测量模块、角速度计算模块、方程确定模块、判断模块和自解锁模块；

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

测量模块测量得到三轴惯性稳定平台***内部相对转动的角度，包括：基座绕外框架本体坐标系的X_p2轴转动的角度β_xk，外框架绕内框架本体坐标系的Y_p1轴转动的角度β_yk，内框架绕台体本体坐标系的Z_p轴转动的角度β_zk；

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

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

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

1)在β_yk≠90°且β_yk≠-90°时，平台内框架和外框架不在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

时，平台内框架和外框架在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

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

自解锁模块在需要框架自解锁才能使平台台体相对惯性空间稳定时，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定。

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

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

自解锁模块中，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定的角度值计算过程为：

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

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

当

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

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

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

的表达式分别为

其中，

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

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

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

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

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

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

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

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

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

测量模块，通过如下方法测量得到三轴惯性稳定平台***内部相对转动角度：

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

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

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

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

三轴惯性稳定平台***的基座与载体固连。

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

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

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

本发明给出的一种三轴惯性平台***框架自解锁方法及***完全覆盖了3个姿态角在任意象限的情况，克服了原有技术在内框架角β_yk＝±90°时的“框架锁定”问题；且本发明虽然在计算环节中存在secβ_yk，但给出了在该奇异点处的框架角稳态值，确保了***仍然稳定而不发散，本发明实现了三轴平台在“框架锁定”时的快速解耦和框架状态切换，可有效隔离载体的角运动，提高了平台台体相对惯性空间稳定的全姿态适应能力。

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

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

Claims (9)

1.一种三轴惯性稳定平台***框架自解锁方法，其特征在于：基于三轴惯性稳定平台***实现，所述稳定平台***包括基座、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系X₁Y₁Z₁、外框架本体坐标系X_p2Y_p2Z_p2、内框架本体坐标系X_p1Y_p1Z_p1和台体本体坐标系X_pY_pZ_p；四个坐标系的原点重合，并且：台体本体坐标系的Z_p轴与内框架本体坐标系的Z_p1轴重合，外框架本体坐标系的Y_p2轴与内框架本体坐标系的Y_p1轴重合，基座本体坐标系的X₁轴与外框架本体坐标系的X_p2轴重合；其中，基座与载体固连，在所述稳定平台***在载体带动下发生内部相对转动时，基座绕外框架本体坐标系的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轴的合成转动角速度；

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

采用解耦计算公式，可得平台台体本体坐标系X_pY_pZ_p三个方向上的角速度为

内框架本体坐标系X_p1Y_p1Z_p1三个方向上的角速度为

外框架本体坐标系X_p2Y_p2Z_p2三个方向上的角速度为

三个框架角角速度为

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

1)在β_yk≠90°且β_yk≠-90°时，平台内框架和外框架不在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

时，平台内框架和外框架在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

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

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

(6)在需要框架自解锁才能使平台台体相对惯性空间稳定时，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定；

实现框架自解锁以保证平台台体相对惯性空间仍然稳定的角度值计算过程为：

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

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

当

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

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

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

的表达式分别为

其中，

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的一种三轴惯性稳定平台***框架自解锁方法，其特征在于：在步骤(6)中，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定的角度值计算过程为：

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

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

当

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

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

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

的表达式分别为

其中，

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

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

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

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

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

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

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

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

所以β_yk的值会增大；β_zk的稳态值为β_zk＝β_zk0-Δβ_xk＝β_zk0+α+β_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轴的合成转动角速度；

方程确定模块采用解耦计算公式，得到平台三个框架角角速度确定方程，具体为：

采用解耦计算公式，可得平台台体本体坐标系X_pY_pZ_p三个方向上的角速度为

内框架本体坐标系X_p1Y_p1Z_p1三个方向上的角速度为

外框架本体坐标系X_p2Y_p2Z_p2三个方向上的角速度为

三个框架角角速度为

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

1)在β_yk≠90°且β_yk≠-90°时，平台内框架和外框架不在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

2)在β_yk＝90°或β_yk＝-90°，以及基座角速度ω_y1＝ω_z1＝0时，平台内框架和外框架在一个平面内，不需要框架自解锁使平台台体相对惯性空间稳定；

3)在β_yk＝90°或β_yk＝-90°，当ω_y1、ω_z1中有一个为非零时，平台内框架和外框架在一个平面内，需要框架自解锁才能使平台台体相对惯性空间稳定；

自解锁模块在需要框架自解锁才能使平台台体相对惯性空间稳定时，由外框架带动内框架相对台体快速发生转动，实现框架自解锁以保证平台台体相对惯性空间仍然稳定；

实现框架自解锁以保证平台台体相对惯性空间仍然稳定的角度值计算过程为：

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

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

当

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

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

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

的表达式分别为

其中，

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

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

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

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

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

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

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

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

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

Images