CN106767747B - 一种基于参量激励的数字式速率积分陀螺控制方法和平台 - Google Patents

Abstract

本发明涉及一种基于参量激励的数字式速率积分陀螺控制方法和平台，其中，方法包括步骤：1)设置速率积分陀螺同向参考量和正交参考量的初始值，以及速率积分陀螺振动能量的预设值；2)采集速率积分陀螺X、Y轴运动输出电压数字量并进行检测部件误差校正；3)将校正后的X、Y轴运动输出电压数字量进行0°和90°相敏解调；4)计算各中间变量；5)计算相敏解调参考信号、参量激励驱动电压相位参考信号和驱动电压相位参考信号；6)计算X、Y轴参量激励驱动电压数字量；7)计算陀螺振动角度的正弦值和余弦值；8)计算X、Y轴驱动电压数字量；9)将相敏解调参考信号进行0°和90°相位变换后作为下一采样时刻的同向参考量和正交参考量，返回步骤2)。

Description

一种基于参量激励的数字式速率积分陀螺控制方法和平台

技术领域

本发明属于传感器检测和控制技术领域，尤其涉及一种具有误差校正功能的基于参量激励的数字式速率积分陀螺控制方法和平台。

背景技术

科氏振动陀螺因其小体积、低功耗、低成本等优势，已经成为十分热门的一个技术领域，也已经有了十分广泛的应用。目前，科氏振动陀螺主要有两个方面的发展趋势，一个是面向大众消费市场的大批量低成本型，另一个是面向军事领域的高精度型。针对科氏振动陀螺发展的第二个要求，需要探索提高科氏振动陀螺精度的方法。

科氏振动陀螺根据输出物理量的不同可以分为速率陀螺和速率积分陀螺。与速率陀螺相比，速率积分陀螺直接测量角度。得益于稳定的标度因数和避免了由角速率数值积分引入角度误差，速率积分陀螺具有实现高精度的潜力；同时速率积分陀螺具有机械上无限的测量范围和高带宽。然而由于材料特性和加工的不确定性，陀螺存在各向异性弹性和各向异性阻尼等非理想特性。在速率积分陀螺中，谐振子自由振动，使得测量精度易受陀螺非理想特性的影响，并且给控制带来了挑战。其中，各向异性弹性和各向异性阻尼是速率积分陀螺的两个主要误差源，各向异性弹性和各向异性阻尼会引起与陀螺主振动相位正交的正交振动，导致角度输出误差，并且如果各向异性弹性和各向异性阻尼过大还会导致速率积分模式的失效。此外，陀螺结构运动检测部件误差、驱动部件误差和电路增益误差也会进一步引起速率积分陀螺的输出误差。

发明内容

针对上述问题，本发明的目的是提供一种基于参量激励的数字式速率积分陀螺控制方法和平台，采用参量激励控制实现谐振子的稳定控制，采用正交振动控制克服各向异性弹性引起的陀螺误差，并且具有误差校正功能，能够提高速率积分陀螺的精度，对速率积分陀螺的研制具有重要意义。

为实现上述目的，本发明采取以下技术方案：一种基于参量激励的数字式速率积分陀螺控制方法，包括以下步骤：

1)设置速率积分陀螺的同向参考量和正交参考量的初始值，同时设置速率积分陀螺的振动能量的预设值；

2)采集速率积分陀螺X轴和Y轴运动输出电压数字量，将X轴和Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X轴和Y轴运动输出电压数字量；

3)根据速率积分陀螺的同向参考量和正交参考量，将校正后的X轴和Y轴运动输出电压数字量分别进行0°和90°相敏解调，得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q；

4)利用X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q；

5)将计算得到的反映驱动频率与陀螺谐振频率之差的变量Ls输入频率控制环中，频率控制环计算输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REFSin2和驱动电压相位参考信号REF Sin3；

6)将计算得到的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量误差ΔE，根据振动能量误差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后分别与X轴和Y轴参量激励驱动信号幅值的数字量相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴参量驱动装置；

7)根据计算得到的反映陀螺振动角度的变量R和S，计算陀螺的振动角度；根据陀螺振动角度，计算陀螺振动角度的正弦值和余弦值；

8)根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后分别与正交抑制驱动电压X轴和Y轴分量的幅值相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴驱动装置；

9)将频率控制环输出的相敏解调参考信号REF Sin1分别进行0°和90°相位变换后作为下一采样时刻速率积分陀螺的同向参考量和正交参考量，返回步骤2)；如此循环控制，实现对速率积分陀螺的闭环控制。

所述步骤2)中的检测部件误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

所述步骤4)中反映驱动频率与陀螺谐振频率之差的变量Ls、反映陀螺振动能量的变量E、反映陀螺振动角度的变量R和S以及反映陀螺正交振动的变量Q的计算公式分别为：

Ls＝2(x_ix_q+y_iy_q)

E＝x_i ²+x_q ²+y_i ²+y_q ²

R＝x_i ²+x_q ²-y_i ²-y_q ²

S＝2(x_iy_i-x_qy_q)

Q＝2(x_iy_q-x_qy_i)。

所述步骤5)中的频率控制环包括驱动信号频率计算模块、相敏解调参考信号REFSin1计算模块、参量激励驱动电压相位参考信号REF Sin2计算模块以及驱动电压相位参考信号REF Sin3计算模块；其中，驱动信号频率计算模块根据反映驱动频率与陀螺谐振频率之差的变量Ls，通过频率控制算法计算驱动信号的频率；相敏解调参考信号REF Sin1计算模块将驱动信号的频率进行相位累加后计算正弦，得到相敏解调参考信号REF Sin1；参量激励驱动电压相位参考信号REF Sin2计算模块将驱动信号的频率进行二倍频，然后将二倍频的驱动信号进行相位累加后计算正弦，得到参量激励驱动电压相位参考信号REF Sin2；驱动电压相位参考信号REF Sin3计算模块将驱动信号的频率进行相位累加后计算正弦，得到驱动电压相位参考信号REF Sin3。

频率控制算法采用的是比例—积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，f(k)为第k采样时刻的驱动信号频率f(k-1)，为第k-1采样时刻的驱动信号频率，K_P1为频率控制算法比例系数，K_I1为频率控制算法积分系数，Δt为采样时间间隔，Ls(k)为第k采样时刻的Ls变量值，Ls(k-1)为第k-1采样时刻的Ls变量值；

相敏解调参考信号REF Sin1的计算公式为：

REF Sin1(k)＝sin(ψ(k))

ψ(k)＝ψ(k-1)+2πf(k-1)Δt

式中，REF Sin1(k)为第k采样时刻的相敏解调参考信号，ψ(k)为第k采样时刻的正弦波相位，ψ(k-1)为第k-1采样时刻的正弦波相位；

参量激励驱动电压相位参考信号REF Sin2的计算公式为：

REF Sin2(k)＝sin(β(k))

β(k)＝β(k-1)+2π2f(k)Δt

式中，REF Sin2(k)为第k采样时刻的参量激励驱动电压相位参考信号，β(k)为第k采样时刻的正弦波相位，β(k-1)为第k-1采样时刻的正弦波相位；

驱动电压相位参考信号REF Sin3的计算公式为：

REF Sin3(k)＝sin(γ(k))

γ(k)＝γ(k-1)+2πf(k)Δt

式中，REF Sin3(k)为第k采样时刻的驱动电压相位参考信号，γ(k)为第k采样时刻的正弦波相位，γ(k-1)为第k-1采样时刻的正弦波相位。

所述步骤6)中的振动能量控制算法采用的是比例—积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，V_P(k)为第k采样时刻的参量激励驱动信号幅值，V_P(k-1)为第k-1采样时刻的参量激励驱动信号幅值，K_P2为振动能量控制算法比例系数，K_I2为振动能量控制算法积分系数，Δt为采样时间间隔，ΔE(k)为第k采样时刻的振动能量误差ΔE的值，ΔE(k-1)为第k-1采样时刻的振动能量误差ΔE的值。

所述步骤6)中的参量驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

所述步骤8)中的正交振动控制算法采用的是比例—积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，V_Q(k)为第k采样时刻的正交抑制驱动电压幅值，V_Q(k-1)为第k-1采样时刻的正交抑制驱动电压幅值，K_P3为正交振动控制算法比例系数，K_I3为正交振动控制算法积分系数，Δt为采样时间间隔，Q(k)为第k采样时刻的Q变量值，Q(k-1)为第k-1采样时刻的Q变量值。

所述步骤8)中的驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

一种基于参量激励的数字式速率积分陀螺控制平台，基于上述的基于参量激励的数字式速率积分陀螺控制方法，其特征在于，该平台包括逻辑控制模块、数模/模数转换模块和处理器；

其中，逻辑控制模块包括定时器、逻辑控制模块数据传输控制单元和数模/模数转换及控制单元，定时器用于定时产生逻辑控制的时钟信号；逻辑控制模块数据传输控制单元用于实现逻辑控制模块与处理器间的高速数据传输；数模/模数转换及控制单元包括数模转换控制单元和模数转换控制单元，用于产生数模/模数转换模块的控制逻辑信号并实现逻辑控制模块与数模/模数转换模块的数据传输；

数模/模数转换模块包括数模转换单元和模数转换单元，数模转换单元用于将控制平台产生的X轴和Y轴驱动电压数字量及X轴和Y轴参量激励驱动电压数字量转化成X轴和Y轴驱动电压及X轴和Y参量激励驱动电压，模数转换单元将X轴和Y轴运动输出电压转化成X轴和Y轴运动输出电压数字量；

处理器包括响应控制单元、处理器数据传输控制单元和控制算法单元；响应控制单元用于根据定时器定时产生的逻辑控制的时钟信号启动控制算法单元运行，同时控制处理器数据传输控制单元进行数据传输；处理器数据传输控制单元用于实现处理器与逻辑控制模块之间的高速数据传输；控制算法单元包括检测部件误差校正部件、相敏解调部件、变量计算部件、频率控制环、参量激励驱动电压计算部件、振动角度计算部件和驱动电压计算部件；

检测部件误差校正部件用于将输入的速率积分陀螺X、Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X、Y轴运动输出电压数字量并输出；

相敏解调部件用于根据同向参考量和正交参考量，将输入的校正后的X、Y轴运动输出电压数字量分别进行0°和90°相敏解调，得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q并输出；

变量计算部件用于利用输入的X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q并输出；

频率控制环用于根据输入的反映驱动频率与陀螺谐振频率之差的变量Ls计算并输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REF Sin2和驱动电压相位参考信号REF Sin3；

参量激励驱动电压计算部件用于将输入的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量误差ΔE，根据振动能量误差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后与X轴和Y轴参量激励驱动信号幅值的数字量分别相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量并输出；

振动角度计算部件用于根据反映陀螺振动角度的变量R和S计算陀螺的振动角度，并根据陀螺振动角度计算陀螺振动角度的正弦值和余弦值并输出；

驱动电压计算部件用于根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后与正交抑制驱动电压X轴和Y轴分量的幅值分别相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量并输出。

本发明由于采取以上技术方案，其具有以下优点：1、本发明的一种基于参量激励的数字式速率积分陀螺控制方法和平台，通过采用参量激励控制实现谐振子的稳定控制，有助于获得更高的标度因数稳定性，通过采用正交抑制能够降低由各向异性弹性导致的陀螺误差，能够有效提高速率积分陀螺的精度。2、本发明的一种基于参量激励的数字式速率积分陀螺控制方法和平台，通过检测部件误差校正、参量驱动装置误差校正和驱动装置误差校正能够降低由结构运动检测部件误差、驱动部件误差和电路增益误差导致的陀螺输出误差，对速率积分陀螺的研制具有重要意义。

附图说明

图1是双轴对称陀螺结构组成图；

图2(a)是参量激励谐振器的工作原理图；

图2(b)是参量激励谐振器的稳定区和不稳定区示意图；

图3是本发明控制方法的流程框图；

图4是本发明的频率控制环算法的流程框图；

图5是本发明控制平台的硬件示意框图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明的控制方法和平台适用于双轴对称的科氏振动陀螺结构，当然也可以应用于其他科氏振动陀螺结构。双轴对称的科氏振动陀螺具有X轴和Y轴两个运动方向，在X轴和Y轴运动方向上均分别设置有检测部件、驱动装置和参量驱动装置；检测部件用于分别检测X轴和Y轴运动位移并转化为X轴和Y轴运动输出电压，分别在X轴和/或Y轴运动方向的驱动装置上施加驱动电压会向振动陀螺施加一个沿X轴和/或Y轴运动方向的驱动力，分别在X轴和/或Y轴运动方向的参量驱动装置上施加参量激励驱动电压会改变振动陀螺在X轴和/或Y轴运动方向的变刚度。例如，如果分别在X轴和Y轴运动方向的参量驱动装置上施加两倍振动频率的参量激励驱动电压，将在X轴和Y轴上产生变化频率为振动频率两倍的变化刚度。

如图2(a)、(b)所示，参量激励谐振由普通质量弹簧阻尼谐振器和可变刚度的弹簧组成，可变刚度通过在参量驱动装置上施加两倍振动频率的参量激励驱动电压产生，可变刚度变化频率为振动频率的两倍，其动力学微分方程为：

式中，m为谐振器质量，x为谐振器的运动位移，和分别为x对时间t的一阶和二阶导数，c为谐振器阻尼系数，k为谐振器刚度，ω为谐振器振动频率，Δk为可变刚度幅度，t为时间。

当谐振器振动频率一定时，谐振器的运动状态由可变刚度幅度Δk决定。存在一个临界值，在Δk等于临界值时谐振器维持当前运动状态；当Δk大于临界值(不稳定区)，谐振器的运动幅度随时间增加；当Δk小于临界值时(稳定区)，谐振器的运动幅度随时间减小直至为零。通过改变参量激励驱动电压(即变化刚度)的大小使科氏振动陀螺振动的幅度增加、减小或保持不变，实现陀螺振动能量的控制。

根据上述原理，如图3所示，本发明提供的一种基于参量激励的数字式速率积分陀螺控制方法，具体包括以下步骤：

1)设置速率积分陀螺的同向参考量和正交参考量的初始值，一般将同向参考量和正交参考量的初始值分别设置为0和1；同时设置速率积分陀螺的振动能量的预设值。

2)采集速率积分陀螺X轴和Y轴运动输出电压数字量，将X轴和Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X轴和Y轴运动输出电压数字量。

检测部件误差校正矩阵的数学形式为2×2矩阵，矩阵每一项可通过实验确定；检测部件误差校正矩阵用于校正由于检测部件将X轴和Y轴运动位移转化为运动输出电压的转化系数不同造成的误差，以及由于X轴和Y轴运动方向的检测部件之间存在耦合(Y轴运动方向的检测部件将X轴运动转化为Y轴运动输出电压和/或X轴运动方向的检测部件将Y轴运动转化为X轴运动输出电压)造成的误差。

3)根据速率积分陀螺的同向参考量和正交参考量，将校正后的X轴和Y轴运动输出电压数字量分别进行0°和90°相敏解调，分别得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q。

4)利用X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q；上述各变量的计算公式如下：

Ls＝2(x_ix_q+y_iy_q)

E＝x_i ²+x_q ²+y_i ²+y_q ²

R＝x_i ²+x_q ²-y_i ²-y_q ²

S＝2(x_iy_i-x_qy_q)

Q＝2(x_iy_q-x_qy_i)

5)将计算得到的反映驱动频率与陀螺谐振频率之差的变量Ls输入频率控制环中，频率控制环计算输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REFSin2和驱动电压相位参考信号REF Sin3。

如图4所示，频率控制环包括驱动信号频率计算模块、相敏解调参考信号REF Sin1计算模块、参量激励驱动电压相位参考信号REF Sin2计算模块以及驱动电压相位参考信号REF Sin3计算模块；其中，驱动信号频率计算模块根据反映驱动频率与陀螺谐振频率之差的变量Ls，通过频率控制算法计算驱动信号的频率；相敏解调参考信号REF Sin1计算模块将驱动信号的频率进行相位累加后计算正弦，得到相敏解调参考信号REF Sin1；参量激励驱动电压相位参考信号REF Sin2计算模块将驱动信号的频率进行二倍频，然后将二倍频的驱动信号进行相位累加后计算正弦，得到参量激励驱动电压相位参考信号REF Sin2；驱动电压相位参考信号REF Sin3计算模块将驱动信号的频率进行相位累加后计算正弦，得到驱动电压相位参考信号REF Sin3。

其中，频率控制算法采用的是比例—积分控制算法，其可表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，f(k)为第k采样时刻的驱动信号频率，f(k-1)为第k-1采样时刻的驱动信号频率，K_P1为频率控制算法比例系数，K_I1为频率控制算法积分系数，Δt为采样时间间隔，Ls(k)为第k采样时刻的Ls变量值，Ls(k-1)为第k-1采样时刻的Ls变量值。

相敏解调参考信号REF Sin1的计算公式为：

REF Sin1(k)＝sin(ψ(k))

ψ(k)=ψ(k-1)+2πf(k-1)Δt

式中，REF Sin1(k)为第k采样时刻的相敏解调参考信号，ψ(k)为第k采样时刻的正弦波相位，ψ(k-1)为第k-1采样时刻的正弦波相位。

参量激励驱动电压相位参考信号REF Sin2的计算公式为：

REF Sin2(k)＝sin(β(k))

β(k)＝β(k-1)+2π2f(k)Δt

式中，REF Sin2(k)为第k采样时刻的参量激励驱动电压相位参考信号，β(k)为第k采样时刻的正弦波相位，β(k-1)为第k-1采样时刻的正弦波相位。

驱动电压相位参考信号REF Sin3的计算公式为：

REF Sin3(k)＝sin(γ(k))

γ(k)＝γ(k-1)+2πf(k)Δt

式中，REF Sin3(k)为第k采样时刻的驱动电压相位参考信号，γ(k)为第k采样时刻的正弦波相位，γ(k-1)为第k-1采样时刻的正弦波相位。

6)将计算得到的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量误差ΔE，根据振动能量误差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后分别与X轴和Y轴参量激励驱动信号幅值的数字量相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴参量驱动装置。

其中，振动能量控制算法采用的是比例—积分控制算法，其可表述如下：

式中，V_P(k)为第k采样时刻的参量激励驱动信号幅值，V_P(k-1)为第k-1采样时刻的参量激励驱动信号幅值，K_P2为振动能量控制算法比例系数，K_I2为振动能量控制算法积分系数，ΔE(k)为第k采样时刻的振动能量误差ΔE的值，ΔE(k-1)为第k-1采样时刻的振动能量误差ΔE的值。

参量驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项可通过实验确定；参量驱动装置误差校正矩阵用于校正由于参量驱动装置将X轴和Y轴参量激励驱动电压转化为X轴和Y轴方向运动变刚度的转化系数不同造成的误差，以及由于X轴和Y轴运动方向的参量驱动装置之间存在耦合(Y轴运动方向的参量驱动装置将X轴方向的参量激励驱动电压转化为Y轴方向运动变刚度和/或X轴运动方向的参量驱动装置将Y轴方向的参量激励驱动电压转化为X轴方向运动变刚度)造成的误差。

7)根据计算得到的反映陀螺振动角度的变量R和S，计算陀螺的振动角度；根据陀螺振动角度，通过旋转矩阵计算与陀螺振动角度垂直方向的单位向量在X轴和Y轴上的投影值，即陀螺振动角度的正弦值和余弦值。

8)根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后分别与正交抑制驱动电压X轴和Y轴分量的幅值相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴驱动装置。

其中，正交振动控制算法采用的是比例—积分控制算法，其可表述如下：

式中，V_Q(k)为第k采样时刻的正交抑制驱动电压幅值，V_Q(k-1)为第k-1采样时刻的正交抑制驱动电压幅值，K_P3为正交振动控制算法比例系数，K_I3为正交振动控制算法积分系数，Q(k)为第k采样时刻的Q变量值，Q(k-1)为第k-1采样时刻的Q变量值。

驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项可通过实验确定；驱动装置误差校正矩阵用于校正由于驱动装置将X轴和Y轴驱动电压转化为X轴和Y轴方向驱动力的转化系数不同造成的误差，以及由于X轴和Y轴驱动装置之间存在耦合(Y轴驱动装置将X轴驱动电压转化为Y轴方向驱动力和/或X轴驱动装置将Y轴驱动电压转化为X轴方向驱动力)造成的误差。

9)将频率控制环输出的相敏解调参考信号REF Sin1分别进行0°和90°相位变换后作为下一采样时刻的速率积分陀螺的同向参考量和正交参考量，返回步骤2)；如此循环控制，实现对速率积分陀螺的闭环控制。

如图5所示，本发明还提供了一种基于参量激励的数字式速率积分陀螺控制平台，包括逻辑控制模块、数模/模数转换模块和处理器。

其中，逻辑控制模块包括定时器、逻辑控制模块数据传输控制单元和数模/模数转换及控制单元，定时器用于定时产生逻辑控制的时钟信号；逻辑控制模块数据传输控制单元用于实现逻辑控制模块与处理器间的高速数据传输；数模/模数转换及控制单元包括数模转换控制单元和模数转换控制单元，用于产生数模/模数转换模块的控制逻辑信号并实现逻辑控制模块与数模/模数转换模块的数据传输。

数模/模数转换模块包括数模转换单元和模数转换单元，数模转换单元用于将控制平台产生的X轴和Y轴驱动电压数字量及X轴和Y轴参量激励驱动电压数字量转化成X轴和Y轴驱动电压及X轴和Y参量激励驱动电压，模数转换单元将X轴和Y轴运动输出电压转化成X轴和Y轴运动输出电压数字量。

处理器包括响应控制单元、处理器数据传输控制单元和控制算法单元；响应控制单元用于根据定时器定时产生的逻辑控制的时钟信号启动控制算法单元运行，同时控制处理器数据传输控制单元进行数据传输；处理器数据传输控制单元用于实现处理器与逻辑控制模块之间的高速数据传输；控制算法单元包括检测部件误差校正部件、相敏解调部件、变量计算部件、频率控制环、参量激励驱动电压计算部件、振动角度计算部件和驱动电压计算部件；

检测部件误差校正部件用于将输入的速率积分陀螺X、Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X、Y轴运动输出电压数字量并输出；

相敏解调部件用于根据同向参考量和正交参考量，将输入的校正后的X、Y轴运动输出电压数字量分别进行0°和90°相敏解调，得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q并输出；

变量计算部件用于利用输入的X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q并输出；

频率控制环用于根据输入的反映驱动频率与陀螺谐振频率之差的变量Ls计算并输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REF Sin2和驱动电压相位参考信号REF Sin3；

参量激励驱动电压计算部件用于将输入的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量差ΔE，根据振动能量差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后与X轴和Y轴参量激励驱动信号幅值的数字量分别相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量并输出；

振动角度计算部件用于根据反映陀螺振动角度的变量R和S计算陀螺的振动角度，并根据陀螺振动角度计算陀螺振动角度的正弦值和余弦值并输出；

驱动电压计算部件用于根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后与正交抑制驱动电压X轴和Y轴分量的幅值分别相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量并输出。

上述各实施例仅用于说明本发明，其中各部件的结构、设置位置及其连接方式等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。

Claims (10)

1.一种基于参量激励的数字式速率积分陀螺控制方法，包括以下步骤：

1)设置速率积分陀螺的同向参考量和正交参考量的初始值，同时设置速率积分陀螺的振动能量的预设值；

2)采集速率积分陀螺X轴和Y轴运动输出电压数字量，将X轴和Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X轴和Y轴运动输出电压数字量；

3)根据速率积分陀螺的同向参考量和正交参考量，将校正后的X轴和Y轴运动输出电压数字量分别进行0°和90°相敏解调，得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q；

4)利用X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q；

5)将计算得到的反映驱动频率与陀螺谐振频率之差的变量Ls输入频率控制环中，频率控制环计算输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REF Sin2和驱动电压相位参考信号REF Sin3；

6)将计算得到的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量误差ΔE，根据振动能量误差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后分别与X轴和Y轴参量激励驱动信号幅值的数字量相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴参量驱动装置；

7)根据计算得到的反映陀螺振动角度的变量R和S，计算陀螺的振动角度；根据陀螺振动角度，计算陀螺振动角度的正弦值和余弦值；

8)根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后分别与正交抑制驱动电压X轴和Y轴分量的幅值相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量，并分别输出给速率积分陀螺的X轴和Y轴驱动装置；

9)将频率控制环输出的相敏解调参考信号REF Sin1分别进行0°和90°相位变换后作为下一采样时刻速率积分陀螺的同向参考量和正交参考量，返回步骤2)；如此循环控制，实现对速率积分陀螺的闭环控制。

2.如权利要求1所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤2)中的检测部件误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

3.如权利要求1或2所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤4)中反映驱动频率与陀螺谐振频率之差的变量Ls、反映陀螺振动能量的变量E、反映陀螺振动角度的变量R和S以及反映陀螺正交振动的变量Q的计算公式分别为：

Ls＝2(x_ix_q+y_iy_q)

E＝x_i ²+x_q ²+y_i ²+y_q ²

R＝x_i ²+x_q ²-y_i ²-y_q ²

S＝2(x_iy_i-x_qy_q)

Q＝2(x_iy_q-x_qy_i)。

4.如权利要求3所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤5)中的频率控制环包括驱动信号频率计算模块、相敏解调参考信号REF Sin1计算模块、参量激励驱动电压相位参考信号REF Sin2计算模块以及驱动电压相位参考信号REF Sin3计算模块；其中，驱动信号频率计算模块根据反映驱动频率与陀螺谐振频率之差的变量Ls，通过频率控制算法计算驱动信号的频率；相敏解调参考信号REF Sin1计算模块将驱动信号的频率进行相位累加后计算正弦，得到相敏解调参考信号REF Sin1；参量激励驱动电压相位参考信号REF Sin2计算模块将驱动信号的频率进行二倍频，然后将二倍频的驱动信号进行相位累加后计算正弦，得到参量激励驱动电压相位参考信号REF Sin2；驱动电压相位参考信号REF Sin3计算模块将驱动信号的频率进行相位累加后计算正弦，得到驱动电压相位参考信号REF Sin3。

5.如权利要求4所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，频率控制算法采用的是比例-积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，f(k)为第k采样时刻的驱动信号频率，f(k-1)为第k-1采样时刻的驱动信号频率，K_P1为频率控制算法比例系数，K_I1为频率控制算法积分系数，Δt为采样时间间隔，Ls(k)为第k采样时刻的Ls变量值，Ls(k-1)为第k-1采样时刻的Ls变量值；

相敏解调参考信号REF Sin1的计算公式为：

REF Sin1(k)＝sin(ψ(k))

ψ(k)＝ψ(k-1)+2πf(k-1)Δt

式中，REF Sin1(k)为第k采样时刻的相敏解调参考信号，ψ(k)为第k采样时刻的正弦波相位，ψ(k-1)为第k-1采样时刻的正弦波相位；

参量激励驱动电压相位参考信号REF Sin2的计算公式为：

REF Sin2(k)＝sin(β(k))

β(k)＝β(k-1)+2π2f(k)Δt

式中，REF Sin2(k)为第k采样时刻的参量激励驱动电压相位参考信号，β(k)为第k采样时刻的正弦波相位，β(k-1)为第k-1采样时刻的正弦波相位；

驱动电压相位参考信号REF Sin3的计算公式为：

REF Sin3(k)＝sin(γ(k))

γ(k)＝γ(k-1)+2πf(k)Δt

式中，REF Sin3(k)为第k采样时刻的驱动电压相位参考信号，γ(k)为第k采样时刻的正弦波相位，γ(k-1)为第k-1采样时刻的正弦波相位。

6.如权利要求1所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤6)中的振动能量控制算法采用的是比例-积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，V_P(k)为第k采样时刻的参量激励驱动信号幅值，V_P(k-1)为第k-1采样时刻的参量激励驱动信号幅值，K_P2为振动能量控制算法比例系数，K_I2为振动能量控制算法积分系数，Δt为采样时间间隔，ΔE(k)为第k采样时刻的振动能量误差ΔE的值，ΔE(k-1)为第k-1采样时刻的振动能量误差ΔE的值。

7.如权利要求1或2或4或5或6所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤6)中的参量驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

8.如权利要求1所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤8)中的正交振动控制算法采用的是比例-积分控制算法，表述如下：

式中，k表示第k采样时刻，k-1表示第k-1采样时刻，V_Q(k)为第k采样时刻的正交抑制驱动电压幅值，V_Q(k-1)为第k-1采样时刻的正交抑制驱动电压幅值，K_P3为正交振动控制算法比例系数，K_I3为正交振动控制算法积分系数，Δt为采样时间间隔，Q(k)为第k采样时刻的Q变量值，Q(k-1)为第k-1采样时刻的Q变量值。

9.如权利要求1或2或4或5或6或8所述的一种基于参量激励的数字式速率积分陀螺控制方法，其特征在于，所述步骤8)中的驱动装置误差校正矩阵的数学形式为2×2矩阵，矩阵每一项通过实验确定。

10.一种基于参量激励的数字式速率积分陀螺控制平台，基于如权利要求1至9中任一项所述的基于参量激励的数字式速率积分陀螺控制方法，其特征在于，该平台包括逻辑控制模块、数模/模数转换模块和处理器；

其中，逻辑控制模块包括定时器、逻辑控制模块数据传输控制单元和数模/模数转换及控制单元，定时器用于定时产生逻辑控制的时钟信号；逻辑控制模块数据传输控制单元用于实现逻辑控制模块与处理器间的高速数据传输；数模/模数转换及控制单元包括数模转换控制单元和模数转换控制单元，用于产生数模/模数转换模块的控制逻辑信号并实现逻辑控制模块与数模/模数转换模块的数据传输；

数模/模数转换模块包括数模转换单元和模数转换单元，数模转换单元用于将控制平台产生的X轴和Y轴驱动电压数字量及X轴和Y轴参量激励驱动电压数字量转化成X轴和Y轴驱动电压及X轴和Y参量激励驱动电压，模数转换单元将X轴和Y轴运动输出电压转化成X轴和Y轴运动输出电压数字量；

处理器包括响应控制单元、处理器数据传输控制单元和控制算法单元；响应控制单元用于根据定时器定时产生的逻辑控制的时钟信号启动控制算法单元运行，同时控制处理器数据传输控制单元进行数据传输；处理器数据传输控制单元用于实现处理器与逻辑控制模块之间的高速数据传输；控制算法单元包括检测部件误差校正部件、相敏解调部件、变量计算部件、频率控制环、参量激励驱动电压计算部件、振动角度计算部件和驱动电压计算部件；

检测部件误差校正部件用于将输入的速率积分陀螺X、Y轴运动输出电压数字量与检测部件误差校正矩阵相乘，得到校正后的X、Y轴运动输出电压数字量并输出；

相敏解调部件用于根据同向参考量和正交参考量，将输入的校正后的X、Y轴运动输出电压数字量分别进行0°和90°相敏解调，得到X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q并输出；

变量计算部件用于利用输入的X轴运动同向分量x_i、X轴运动正交分量x_q、Y轴运动同向分量y_i以及Y轴运动正交分量y_q分别计算反映驱动频率与陀螺谐振频率之差的变量Ls，反映陀螺振动能量的变量E，反映陀螺振动角度的变量R和S，以及反映陀螺正交振动的变量Q并输出；

频率控制环用于根据输入的反映驱动频率与陀螺谐振频率之差的变量Ls计算并输出相敏解调参考信号REF Sin1、参量激励驱动电压相位参考信号REF Sin2和驱动电压相位参考信号REF Sin3；

参量激励驱动电压计算部件用于将输入的反映陀螺振动能量的变量E与速率积分陀螺的振动能量的预设值进行比较，得到振动能量误差ΔE，根据振动能量误差ΔE通过振动能量控制算法计算X轴和Y轴参量激励驱动信号幅值的数字量；将参量激励驱动电压相位参考信号REF Sin2进行角度的相位变换，然后与X轴和Y轴参量激励驱动信号幅值的数字量分别相乘，得到X轴和Y轴参量激励驱动信号中间值；再将X轴和Y轴参量激励驱动信号中间值与参量驱动装置误差校正矩阵相乘，得到X轴和Y轴参量激励驱动电压数字量并输出；

振动角度计算部件用于根据反映陀螺振动角度的变量R和S计算陀螺的振动角度，并根据陀螺振动角度计算陀螺振动角度的正弦值和余弦值并输出；

驱动电压计算部件用于根据反映陀螺正交振动的变量Q，通过正交振动控制算法计算正交抑制驱动电压幅值的数字量，将正交抑制驱动电压幅值的数字量与陀螺振动角度的正弦值和余弦值分别相乘，得到正交抑制驱动电压X轴和Y轴分量的幅值；将驱动电压相位参考信号REF Sin3进行0°相位变换，然后与正交抑制驱动电压X轴和Y轴分量的幅值分别相乘，得到X轴和Y轴驱动信号中间值；再将X轴和Y轴驱动信号中间值与驱动装置误差校正矩阵相乘，得到X轴和Y轴驱动电压数字量并输出。