一种三轴惯性稳定平台系统伺服回路变增益控制方法与流程

本发明涉及一种三轴稳定平台系统伺服回路变增益控制方法，尤其涉及三轴平台系统的伺服回路控制方法，主要用于实现全姿态高精度导航的航空、航天领域。

背景技术：

三轴惯性稳定平台系统能有效隔离运动载体的角运动扰动，使得惯性测量单元相对于惯性空间保持稳定，其包括台体、内框架、外框架和基座，其中，惯性测量单元安装在台体内，基座固连在运动载体上。其中，内框架和外框架组成的框架系统，用于为台体提供相对基座的旋转自由度，但由于框架系统和台体间存在着相对运动约束，所以框架系统的运动会对台体带来影响。这些影响包含基座与台体间的坐标变换、力矩变换，以及框架系统惯性干扰力矩对台体的作用等。

在惯性稳定平台伺服系统工作时，框架系统的转动惯量最终通过惯性干扰力矩对台体的作用体现出来，包括转动惯量间的耦合、惯量积的耦合等，但最关键的是框架转动惯量在台体上的耦合。另外，为使台体相对惯性空间稳定，在台体上安装了三个陀螺仪作为伺服回路的敏感元件，但由于作为伺服回路执行机构的三个力矩电机分别装在台体轴、内框架轴和外框架轴上，在不同框架角时伺服回路存在着耦合。βyk为外框架绕内框架坐标系yp1轴转动的角度；βzk为内框架绕台体坐标系zp轴转动的角度。

现有技术中都是取βyk、βzk为零时计算的转动惯量常值作为设计输入，这会带来以下两个问题：1.βyk、βzk为非零时的转动惯量的变化会导致系统的带宽变化，从而引起幅值裕度和相位裕度变化，影响伺服回路的稳定性；2.βyk、βzk为非零时的转动惯量的变化会引起伺服回路的静态精度和动态精度，因此，急需开展三轴惯性稳定平台系统伺服回路控制方法研究。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种三轴惯性稳定平台系统伺服回路变增益控制方法，该方法可实现框架角变化时伺服回路性能保持不变，可以有效提高台体相对惯性空间的稳定精度和能力。

本发明的上述目的通过以下技术方案实现：一种三轴惯性稳定平台系统伺服回路变增益控制方法，其特征在于：基于三轴惯性稳定平台系统实现，所述稳定平台系统包括基座、外框架、内框架和台体，其中，外框架通过外框架轴与基座连接，内框架通过内框架轴与外框架连接，台体通过台体轴与内框架连接，对应的本体坐标系分别为基座本体坐标系x1y1z1、外框架本体坐标系xp2yp2zp2、内框架本体坐标系xp1yp1zp1和台体本体坐标系xpypzp；所述四个坐标系的原点重合，并且：台体本体坐标系的zp轴与内框架本体坐标系的zp1轴重合，外框架的本体坐标系的yp2轴与内框架本体坐标系的yp1轴重合，基座本体坐标系的x1轴与外框架本体坐标系的xp2轴重合；其中，基座与载体固定连接，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕外框架坐标系的xp2轴转动，外框架绕内框架坐标系的yp1轴转动，内框架绕台体坐标系的zp轴转动；

所述三轴惯性稳定平台系统伺服回路变增益控制方法包括如下步骤：

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

(2)将台体在zp轴上的角速度分量经过解耦后得到台体zp轴的合成转动角速度ωz；将台体在yp轴上的角速度分量经过解耦后得到内框架yp1轴的合成转动角速度ωy；将台体在xp轴上的角速度分量经过解耦后得到外框架xp2轴的合成转动角速度ωx；

(3)得到外框架绕内框架本体坐标系的yp1轴转动的角度βyk＝0并且内框架绕台体本体坐标系的zp轴转动的角度βzk＝0时的转动惯量：

其中，分别为台体相对于xp轴、yp轴、zp轴的转动惯量；分别为内框架相对于xp1轴、yp1轴轴的转动惯量；分别为外框架相对于xp2轴、yp2轴的转动惯量，为βyk＝0、并且βzk＝0时的台体本体坐标系的xp轴合成转动惯量，为βyk＝0并且、βzk＝0时的台体本体坐标系的yp轴合成转动惯量，为βyk＝0并且、βzk＝0时的台体本体坐标系的zp轴合成转动惯量；

(4)分别把作为xp轴、yp轴和zp轴伺服回路的被控对象，根据性能指标要求设计控制器，得到台体本体坐标系的xp轴控制器为cx,0(s)、台体本体坐标系的yp轴控制器为cy,0(s)、台体本体坐标系的zp轴控制器为的控制器分别为cx,0(s)、cy,0(s)、cz,0(s)；其中，为力矩电机的传递函数，t为电机的时间常数，k0为电机的输出比例系数；ux、uy、uz分别为xp轴、yp轴和zp轴伺服回路力矩电机的输入量；s为复变角频率，ωx(s)为xp轴角速度ωx(t)的拉氏变换，ux(s)为xp轴控制器输出量ux(t)的拉氏变换，ωy(s)为yp轴角速度ωy(t)的拉氏变换，uy(s)为yp轴控制器输出量uy(t)的拉氏变换，ωz(s)为zp轴角速度ωz(t)的拉氏变换，uz(s)为zp轴控制器输出量uz(t)的拉氏变换；t为时间变量；

(5)当基座相对于台体处于任意姿态时，实时测量出βyk和βzk的值，计算出||jp||的值；结合步骤(4)的台体本体坐标系的xp轴控制器为cx,0(s)、台体本体坐标系的yp轴控制器为cy,0(s)、台体本体坐标系的zp轴控制器为cz,0(s)，得到一组新的三轴平台伺服回路变增益控制器：

其中，cx(s)为根据测量角度βyk、和βzk计算的xp轴控制器，cy(s)为根据测量角度βyk、和βzk计算的yp轴控制器，cz(s)为根据测量角度βyk、和βzk计算的zp轴控制器，jp为三轴惯性稳定平台系统的台体合成转动惯量矩阵。

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(2)中，解耦的步骤为：

(21)得到三轴惯性稳定平台系统内部相对转动的角度，包括：外框架绕内框架坐标系yp1轴转动的角度βyk；内框架绕台体坐标系zp轴转动的角度βzk；

(22)得到三轴惯性稳定平台系统的转动惯量，包括：台体相对于xp轴、yp轴、zp轴的转动惯量的内框架相对于xp1轴、yp1轴、zp1轴的转动惯量的外框架相对于xp2轴、yp2轴、zp2轴的转动惯量的

(23)得到三轴惯性稳定平台系统的台体合成转动惯量矩阵jp，包括：合成到台体xp轴上的主转动惯量合成到台体yp轴上的主转动惯量对xp轴和yp轴的合成转动惯量积jxy、对xp轴和zp轴的合成转动惯量积jxz、对yp轴和zp轴的合成转动惯量积jyz；具体计算公式如下：

式中，

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

写为分量表达式，为

其中，ωz为台体zp轴的合成转动角速度；ωy为内框架yp1轴的合成转动角速度；ωx为外框架xp2轴的合成转动角速度。

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(4)中，性能指标要求包括静态精度指标、动态精度指标和对象不确定性指标；其中，

动态精度指标为：

其中，w1(s)为性能界函数，m为干扰力矩，φmax为台体相对惯性空间的最大转角误差，j为对应轴的转动惯量；

对象不确定性指标为：

其中，w2(s)为权函数。

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(4)中，cx,0(s)、cy,0(s)、cz,0(s)的典型值为：

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(5)中，三轴惯性稳定平台系统内部相对转动角度通过如下方法得到：

在内框架的yp1轴上安装角度传感器，测量得到外框架绕内框架本体坐标系的yp1轴转动的角度βyk；在台体zp轴上安装角度传感器，测量得到内框架绕台体本体坐标系的zp轴转动的角度βzk。

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(3)中，当三轴惯性稳定平台系统的结构确定后，通过有限元分析方法计算得到所述平台的转动惯量，或对所述三轴惯性稳定平台系统进行测量得到转动惯量。

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(5)中，||jp||取1-范数、2-范数或∞-范数，具体计算公式为：

上述三轴惯性稳定平台系统伺服回路变增益控制方法中，在步骤(5)中，外框架绕内框架本体坐标系的yp1轴转动的角度βyk的取值范围为0～360°；内框架绕台体本体坐标系的zp轴转动的角度βzk的取值范围为0～360°。

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

(1)、本发明克服了在全姿态条件下βyk、βzk为非零时转动惯量的变化引起系统带宽变化而引起幅值裕度和相位裕度变化造成伺服回路稳定性下降的问题，实现了全姿态条件下的鲁棒稳定性；

(2)、本发明克服了在全姿态条件下βyk、βzk为非零时转动惯量的变化引起伺服回路的静态精度和动态精度下降的问题，实现了全姿态条件下的精度保持能力；

(3)、本发明在计算过程不存在无解区域，可以覆盖任意姿态角的情况，相比现有的计算方法更准确、适用性更广。

附图说明

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

图2为本发明的三轴惯性稳定平台系统的伺服回路解耦方法流程图；

图3为采用本发明的平台系统伺服回路流程图；

图4为解耦后的平台系统伺服回路流程图；

图5为本发明示例中三轴平台的转动惯量矩阵范数||jp||2在βyk、βzk为±180°的范围内的值；

图6为本发明示例中伺服回路开环伯德图；

图7为本发明示例中伺服回路开环传递函数变化时的伯德图；

图8为本发明示例中伺服回路单位阶跃响应曲线。

具体实施方式

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

一种三轴惯性稳定平台系统的伺服回路解耦方法，基于三轴惯性稳定平台系统实现，所述稳定平台系统包括基座、外框架、内框架和台体，对应的本体坐标系分别为基座本体坐标系x1y1z1、外框架本体坐标系xp2yp2zp2、内框架本体坐标系xp1yp1zp1和台体本体坐标系xpypzp；所述四个坐标系的原点重合，并且：台体本体坐标系的zp轴与内框架本体坐标系的zp1轴重合，外框架的本体坐标系的yp2轴与内框架本体坐标系的yp1轴重合，基座本体坐标系的x1轴与外框架本体坐标系的xp2轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕外框架坐标系的xp2轴转动，外框架绕内框架坐标系的yp1轴转动，内框架绕台体坐标系的zp轴转动；

如图1所示的四个坐标系的关系示意图，这四个本体坐标系的原点重合，并且存在如下相对约束关系：台体本体坐标系的zp轴与内框架本体坐标系的zp1轴重合，外框架的本体坐标系的yp2轴与内框架本体坐标系的yp1轴重合，基座本体坐标系的x1轴与外框架本体坐标系的xp2轴重合；其中，基座与载体固连，在所述稳定平台系统在载体带动下发生内部相对转动时，基座绕外框架坐标系的xp2轴转动且转动角度为βxk，外框架绕内框架坐标系的yp1轴转动且转动角度为βyk，内框架绕台体坐标系的zp轴转动且转动角度为βzk。

如图2所示，该三轴惯性稳定平台系统伺服回路变增益控制方法的步骤如下：

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

(2)、经过解耦后，得到台体zp轴的合成转动角速度ωz、内框架yp1轴的合成转动角速度ωy；外框架xp2轴的合成转动角速度ωx；

(3)、计算或测量出框架归零(βyk＝0、βzk＝0)时的转动惯量：

式中，为台体相对于xp轴、yp轴、zp轴的转动惯量；为内框架相对于xp1轴、yp1轴轴的转动惯量；为外框架相对于xp2轴、yp2轴的转动惯量；

(4)、分别把作为xp轴、yp轴和zp轴伺服回路的被控对象，根据性能指标要求进行控制器的设计，求得的控制器分别为cx,0(s)、cy,0(s)、cz,0(s)；式中，为力矩电机的传递函数，t为电机的时间常数，k0为电机的输出比例系数；ux、uy、uz为xp轴、yp轴和zp轴伺服回路力矩电机的输入量；

(5)、当基座相对于平台台体处于任意姿态时，实时测量出βyk、βzk的值，计算出||jp||的值；结合步骤(4)的控制器cx,0(s)、cy,0(s)、cz,0(s)，可得到一组新的三轴平台伺服回路变增益控制器

在设计三轴惯性稳定平台系统的伺服回路控制器时，需要进行解耦，如图3所示。在图中，第一个解耦器tj实现转动惯量的解耦，第二个解耦器tm实现力矩的解耦

式中，

为台体相对于xp轴、yp轴、zp轴的转动惯量；为内框架相对于xp1轴、yp1轴、zp1轴的转动惯量；为外框架相对于xp2轴、yp2轴、zp2轴的转动惯量；βyk为外框架绕内框架坐标系yp1轴转动的角度；βzk为内框架绕台体坐标系zp轴转动的角度。

||jp||的计算方法可取1-范数、2-范数或∞-范数，具体计算公式为

在经过转动惯量解耦、力矩解耦后，可把平台系统伺服回路简化为三个相对独立的单输入单输出回路，如图4所示。但由于在全姿态条件下，||jp||的值并不是常值，而是一个随着框架角变化的量，如图5所示。

在步骤(2)中的解耦步骤为：

(21)、测量得到所述稳定平台系统内部相对转动的角度，包括：外框架绕内框架坐标系yp1轴转动的角度βyk；内框架绕台体坐标系zp轴转动的角度βzk；

(22)、测量或计算得到三轴惯性稳定平台系统的转动惯量，包括：台体相对于xp轴、yp轴、zp轴的转动惯量的内框架相对于xp1轴、yp1轴、zp1轴的转动惯量的外框架相对于xp2轴、yp2轴、zp2轴的转动惯量的

(23)、计算所述稳定平台系统的台体合成转动惯量矩阵jp，包括：合成到台体xp轴上的主转动惯量合成到台体yp轴上的主转动惯量对轴xp和yp的合成转动惯量积jxy、对轴xp和zp的合成转动惯量积jxz、对轴yp和zp的合成转动惯量积jyz；具体计算公式如下：

式中，

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

写为分量表达式，为

其中，ωz为台体zp轴的合成转动角速度；ωy为内框架yp1轴的合成转动角速度；ωx为外框架xp2轴的合成转动角速度。

在步骤(4)中，性能指标要求包括静态精度指标、动态精度指标和对象不确定性指标：

(1)静态精度指标为伺服回路为一个ii型系统，因此，需增加一个积分环节；

(2)动态精度主要考虑在低频段尽量增加伺服回路的力矩刚度，性能界函数为

式中，m为干扰力矩，φmax为台体相对惯性空间的最大转角误差，j为对应轴的转动惯量；

精度指标为

(3)对象不确定性主要考虑高频未建模特性，权函数为

对象不确定性指标为

由于精度指标和对象不确定性指标是一对矛盾量，因此，在设计控制器时需要在上述两个指标之间取折中。

在步骤(4)中，控制器cx,0(s)、cy,0(s)、cz,0(s)的典型值为：

在步骤(5)中，通过如下方法测量得到三轴惯性稳定平台系统内部相对转动角度：在内框架的yp1轴上安装角度传感器，测量得到外框架绕内框架本体坐标系的yp1轴转动的角度βyk；在台体zp轴上安装传感器测量内框架绕台体本体坐标系的zp轴转动的角度βzk。

在步骤(3)中，当三轴惯性稳定平台系统的结构确定后，通过有限元分析方法计算得到所述平台的转动惯量，或对所述三轴惯性稳定平台系统进行测量得到转动惯量。

在步骤(5)中，||jp||的可取1-范数、2-范数或∞-范数，具体计算公式为

在步骤(5)中，转动角度βyk、βzk的取值范围为0～360°。

为形象说明本发明提供的基于克雷洛夫角奇异条件下的姿态角解算方法，优选的实施例为：

例1：设三轴平台的转动惯量矩阵范数||jp||2在βyk、βzk为±180°的范围内的值如图5所示，其中，当βyk、βzk为0°时的从图中可以看出，随着βyk从0°至90°变化时，||jp||2会逐渐变大，当βyk＝90°、βzk＝0°时，

由于||jp||2为一个与框架角相关的变量，为此，在设计控制器时，先取βyk、βzk为0°时的转动惯量作为常值，以xp伺服回路为例，设计的控制器为

伺服回路开环传递函数如图6所示，可以看出，在带宽范围内的低频段，开环传递函数幅值大于性能界函数的值，而在带宽范围外的高频段，开环传递函数幅值小于对象不确定性的值；伺服回路的带宽为163.2rad/s，相位裕度为53.9°，满足设计要求。

当βyk＝90°、βzk＝0°时，伺服回路带宽减小为157.3rad/s，相位裕度为51.8°，如图7中虚线所示。从图中可以看出，伺服回路仍然稳定，但由于增益下降会引起精度下降、快速响应能力变弱。如图8所示，图中虚线为βyk＝90°、βzk＝0°时无增益补偿时的伺服回路单位阶跃响应，实现为本发明变增益控制的伺服回路单位阶跃响应，可以看出，本发明变增益控制具有响应时间快、超调量小、精度高的优点。

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

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