CN112697123A - 一种半球谐振陀螺的工作模式切换控制方法及系统 - Google Patents

Abstract

本发明涉及半球谐振陀螺控制技术领域，具体是一种半球谐振陀螺的工作模式切换控制方法及系统；所述控制方法包括计算出当前半球谐振陀螺的角加速度并进行滤波；若角加速度＞0且角速度绝对值＜第一判断阈值，或者角加速度≤0且角速度绝对值＜第二判断阈值，则进入力平衡模式；按照力平衡PID控制器施加控制力C_p，使谐振子的驻波位置不动，计算和输出角速度；若角加速度≤0且角速度绝对值≥第二判断阈值，或者角加速度＞0且角速度绝对值≥第一判断阈值，则进入全角模式；按照力平衡PID控制器切断力平衡回路所施加的控制力使得C_p＝0，计算和输出角速度增量；本发明依据载体输入角速度的大小实现了力平衡模式和全角模式的混合控制。

Description

一种半球谐振陀螺的工作模式切换控制方法及系统

技术领域

本发明涉及半球谐振陀螺控制技术领域，具体是一种半球谐振陀螺的工作模式切换控制方法及系统。

背景技术

陀螺仪是惯性导航系统的核心器件之一，其作用是测量载体在惯性空间的角运动。半球谐振陀螺是一种新型惯导级固体陀螺仪，具有体积小、成本低、寿命长、可靠性高等优点，可以广泛应用在航空、航天、车辆、工业机器人导航仪、无人装置姿态性能检测仪和姿态实时校准仪、稳定平台、机场安防等诸多领域。

目前半球谐振陀螺的工作模式有两种：全角模式和力平衡模式。当陀螺工作于全角模式时，谐振子驻波自由进动，通过驻波进动角度来敏感载体在惯性空间的角运动；全角控制模式下陀螺的特点是：标度因数恒定，具有较大的动态测量范围和较大的带宽，但测量精度较低。当陀螺工作于力平衡模式时，谐振子的驻波通过力平衡控制回路实时改变激励力的大小使其不发生偏转，激励力的大小与陀螺输入角速度成比例；力平衡模式下陀螺的特点是：具有噪声低、测量精度高的优点，但测量范围较小。这限制了力平衡模式半球陀螺在具有大机动特点的应用环境中的使用。

如何使陀螺既具备力平衡模式下的高测量精度，又具备全角模式下宽测量范围和稳定刻度因数成为一个亟待解决的技术问题。

发明内容

本发明提供了一种半球谐振陀螺的工作模式切换控制方法及系统，当载体输入角速度较小时采用力平衡模式，保证陀螺的低噪声高精度要求。随着角速度增大，当超过一定阈值时采用全角控制模式，保证陀螺具备稳定的标度因数和大动态测量范围。

具体的，本发明采用以下几方面的技术方案以解决上述技术问题：

在本发明的第一方面，本发明提供了一种半球谐振陀螺的工作模式切换控制方法，所述方法包括：

S1、计算出当前半球谐振陀螺的角加速度并进行滤波；

S2、若角加速度＞0且角速度的绝对值＜第一判断阈值，或者角加速度≤0且角速度的绝对值＜第二判断阈值，则进入力平衡模式；

S3、在力平衡模式下，按照力平衡PID控制器施加控制力C_p，使谐振子的驻波位置不动，实时计算和输出角速度，并返回步骤S1；

S4、若角加速度≤0且角速度的绝对值≥第二判断阈值，或者角加速度＞0且角速度的绝对值≥第一判断阈值，则进入全角模式；

S5、在全角模式下，按照力平衡PID控制器切断力平衡回路所施加的控制力使得C_p＝0，实时计算和输出角速度增量，并返回步骤S1。

在本发明的第二方面，本发明还提供了一种半球谐振陀螺的工作模式切换控制系统，所述控制系统包括由谐振子、检测电极和激励电极构成的半球谐振陀螺；通过电容/电压转换模块将检测电极的电容变换量转换为电压信号；通过信号调理模块、差分放大模块、信号解调模块以及信号解算模块解算后得到力平衡模式下的角速度或者全角模式下的角度变化量和角加速度；根据力平衡模式下的角速度或者全角模式下的角度变化量和角加速度切换模式控制开关，所述模式控制开关断开时所述控制系统处于全角模式；所述模式控制开关闭合时所述控制系统处于力平衡模式，所述模式控制开关连接有力平衡PID控制器，所述力平衡PID控制器输出施加控制力C_p，经过差分驱动器后通过激励电极使得驻波位置不动。

本发明的有益效果：

本发明依据载体角加速度方向、输入角速度大小和角速度阈值之间的关系，通过模式切换开关来判断谐振子所处于的控制模式，并按照判断结果切换出所对应的控制模式。

本发明是依据控制系统内部所计算的载体角速度大小，无需外界干涉，即可实现模式间的无缝自动切换，第一判断阈值和第二判断阈值可依据使用场景改变而重新设置。

本发明将角加速度绝对值与第一判断阈值和第二判断阈值的大小进行判断，相当于设置了模式切换滞回曲线，避免了两种控制模式在切换点因为误差存在造成的意外切换。

本发明依据载体输入角速度的大小实现力平衡模式和全角模式的混合控制，使半球谐振陀螺既具备力平衡控制模式下的低噪声高精度特性，又具备全角控制模式下的大动态测量范围和稳定的标度因数的优点。

附图说明

图1为半球谐振陀螺工作原理图；

图2为驻波运动轨迹图；

图3为本发明的一种半球谐振陀螺的工作模式切换控制流程图；

图4为本发明的一种半球谐振陀螺的工作模式切换控制系统的实现框图；

图5为本发明的一种半球谐振陀螺的工作模式切换控制系统的优选控制流程图；

图中，10、腹驻波，11、波节点，12、波幅点，13、参考点，21、椭圆形，22、x测量轴，23、y测量轴，24、主轴方向的振动模态的波长、25、正交轴方向的振动模态的波长，26、振动相位，27、驻波进动角，301、谐振子，302、检测电极，303、激励电极，304、x路电容/电压转换模块，305、y路电容/电压转换模块、306、x路信号调理模块，307、y路信号调理模块，308、309、差分放大模块，310、信号解调模块，311、信号解算模块，312、控制模式开关、313、正交控制PID，314、幅度控制PID，315、力反馈PID控制器，316、相位控制PID，317、318、319、320差分驱动器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

半球谐振陀螺是一种具有惯导级性能的哥式振动陀螺仪，它采用轴对称或者近似轴对称谐振器，当外界有角速度输入时，由于哥氏惯性力的作用，半球壁的挠性振动主阵型即四波腹驻波10自振频率发生裂解，致使驻波即相对壳体又相对惯性空间进动，其进动角与旋转角速度成反比，其工作原理如图1所示。图1中，四波腹驻波包含四个波节点11和四个波幅点12。从图1中可以看出，当参考点13转动90゜后，驻波10相对惯性空间反相转动27゜。

由于轴对称谐振器受迫振动时的主振动模型是一个四波腹驻波，因此半球谐振陀螺振动模型可以看作是一个二维弹簧质量系统模型，它具有两个振动模态(主轴和正交轴)，其振动方程可表达为：

其中，x,y为驻波两个振动方向上的振动位移，m为谐振子质量，α为驻波进动因子，ω₀为谐振子振动自然频率，Ω为载体旋转角速度；f_x，f_y为维持谐振子振动模型为理想谐振子振动模型而施加的控制力，控制力f_x，f_y可描述为式(2)：

f_x＝f_cxcosφ+f_sxsinφ；f_y＝f_cycosφ+f_sysinφ (2)

该振动方程的通解可表述为：

其中，

φ(t)＝ω₀t+φ'。该方程的通解有明显的物理意义，其驻波的运动轨迹如图2所示。

在公式(3)和图2中，在x测量轴22和y测量轴23构成的坐标系中驻波点的运动轨迹可以参考如图2所示的椭圆形21，在这个运动轨迹中，主轴方向的振动模态的波长24为a，正交轴方向的振动模态的波长25为q，振动相位26表示为φ(t)，驻波进动角27即驻波与第一测量轴之间的夹角为2θ。通过检测驻波进动角θ或通过施加控制力平衡驻波进动角速度即可获得载体相对惯性空间的角运动。

将式(3)带入式(1)并忽略小量

和

后可得：

令

结合式(4)和图2可知，若施加与谐振子振动长轴方向一致，且与振动速度相位一致的力C_a，则可改变谐振子的振动幅度a；施加与谐振子振动正交方向一致，且与振动位移相位一致的力C_q，则可改变谐振子正交方向的振动幅度q。从式(5)可以看出，谐振子频率的变化δω与角速度的变化即角加速度

因为谐振子正交方向的振动幅度q的存在而相互耦合，若施加控制力C_q使谐振子正交方向的振动幅度为零，则δω与

解耦。式(5)可简化为：

令C_r＝δω，

则施加与振动长轴方向一致，且与振动位移相位一致的力C_r，可改变谐振子的振动频率；施加与振动正交方向一致，且与振动速度相位一致的力C_p，可改变驻波进动角的角速度。

综上可知，控制力C_a可改变振动长轴方向的振动幅度，C_q可改变振动正交方向的振动幅度，C_r可改变谐振子振动频率，C_p为可改变驻波方位角。

在上述分析下，本发明提出了一种半球谐振陀螺的工作模式切换控制方法，如图3所示，所述方法包括：

S1、计算出当前半球谐振陀螺的角加速度并进行滤波；

在初始时刻，可以让半球谐振陀螺起振；由于谐振子在实际上加工过程中由于存在制造缺陷，其振动过程与理想谐振子有所差别，例如由于阻尼的存在，谐振子的振动幅度会衰减，需要持续施加一定的控制力C_a维持谐振子振动幅度a恒定；由于谐振子不完全对称，振动频率会随时间发生一定变化，需要施加控制力C_r来抑制这种变化。因此需要对谐振器建立控制约束条件，自动调整控制力来补偿扰动，以确保a为常数，q为零，振动频率和相位与参考信号一致。实际电路通过正交控制、幅度控制和相位控制使实际谐振子的振动模态逼近理想状态。若实际谐振子的振动模态接近理想状态时，若令式(6)中的C_p＝0，则谐振子的驻波处于自由进动状态，即为全角控制模式；若施加一定的控制力C_p，使谐振子的驻波位置不动，即使

为零，此时的控制力C_p大小与陀螺输入角速度成比例，即为力平衡模式。因此控制是否施加控制力C_p以及力C_p的大小即可实现全角模式与力平衡模式的切换，但不管哪种模式，都要通过正交控制、幅度控制和相位控制回路来自动调整相应控制力的大小来补偿由于加工制造等误差及使用环境变化等造成的谐振子振动幅度、正交振动幅度及频率的扰动，使谐振子振动逼近理想振动模态。

因此，在本发明的优选实施例中，本发明还通过正交控制PID施加控制力C_a控制谐振子长轴方向的振动幅度；通过幅度控制PID施加控制力C_q控制谐振子正交方向的振动幅度；通过相位控制PID施加控制力C_r控制谐振子的振动频率；从而自动调整相应控制力C_a、C_q和C_r的大小来补偿扰动。

S2、若角加速度＞0且角速度的绝对值＜第一判断阈值，或者角加速度≤0且角速度的绝对值＜第二判断阈值，则进入力平衡模式；

若角速度在增大，则判断角速度的绝对值大小是否小于模式转换阈值ω_s与模式切换滞回值ω_th之和，这两个值之和即为本发明的第一判断阈值ω₁，表示为ω₁＝ω_s+ω_th。若小于第一判断阈值ω₁，则进入力平衡模式。

另一方面，若角速度减小或者不变，则判断角速度的绝对值大小是否小于模式转换阈值ω_s与模式切换滞回值ω_th之差，这两个值之差即为本发明的第二判断阈值ω₂，表示为ω₂＝ω_s-ω_th。若小于第二判断阈值ω₂，则仍然进入力平衡模式。

S3、在力平衡模式下，按照力平衡PID控制器施加控制力C_p，使谐振子的驻波位置不动，实时计算和输出角速度，并返回步骤S1；

在步骤S3中，需要施加控制力C_q使谐振子的驻波位置不动，输出此控制力，此控制力C_q大小反比于载体旋转角速度。

本实施例结合力平衡回路施力大小C_q与半球谐振陀螺标度因数，计算出陀螺相对惯性空间的旋转角速度。

S4、若角加速度≥0且角速度的绝对值≥第二判断阈值，或者角加速度＜0且角速度的绝对值≥第一判断阈值，则进入全角模式；

与进入力平衡模式刚好相反，若角速度在减小或者不变，则判断角速度的绝对值大小是否大于等于模式转换阈值ω_s与模式切换滞回值ω_th之和。若大于等于第一判断阈值ω₁，则进入全角模式。

另一方面，若角速度在增大，则判断角速度的绝对值大小是否大于等于模式转换阈值ω_s与模式切换滞回值ω_th之差。若大于等于第二判断阈值ω₂，则仍然进入全角模式。

在上述实施例中，模式转换阈值ω_s也可依据实际应用环境进行重新设置；模式切换滞回值ω_th可依据实际系统角速度噪声水平进行更改，ω_th一般为0.1゜/s～0.3゜/s；本发明实施例可以取0.2゜/s。

S5、在全角模式下，按照力平衡PID控制器切断力平衡回路所施加的控制力使得C_p＝0，实时计算和输出角速度增量，并返回步骤S1。

在全角模式下，切断力平衡回路施加的控制力，则可以使得C_p＝0；这里的力平衡回路指的是力平衡PID控制器所构成的力反馈回路。

本实施例可以直接利用驻波进动角θ，结合半球谐振陀螺的标度因数计算出陀螺相对惯性空间单位时间内角度的变化量，即角速度增量。

不管是全角模式还是力平衡模式，步骤S3和步骤S5计算出角速度后，都需要返回步骤S1计算出角加速度，如此周而复始下去，完成力平衡模式和全角模式这两种混合模式控制下的陀螺角速度的测量。

在上述控制方法的支撑下，本发明提供了一种半球谐振陀螺的工作模式切换控制系统，如图4所示，所述控制系统包括由谐振子301、检测电极302和激励电极303构成的半球谐振陀螺；通过电容/电压转换模块将检测电极的电容变换量转换为电压信号；通过信号调理模块，差分放大模块，信号解调模块310以及信号解算模块311解算后得到力平衡模式下的角速度Ω或者全角模式下的角度变化量

和角加速度；根据力平衡模式下的角速度或者全角模式下的角度变化量和角加速度切换模式控制开关312，所述模式控制开关312断开时所述控制系统处于全角模式；所述模式控制开关闭合时所述控制系统处于力平衡模式，所述模式控制开关连接有力平衡PID控制器315，所述力平衡PID控制器315输出施加控制力C_p，经过差分驱动器318后通过激励电极使得驻波位置不动。

更进一步的，所述信号解算模块还解算出长轴方向的振动幅度a、正交方向的振动幅度q和谐振子的振动频率ω，通过正交控制PID313输出控制力C_a，通过差分驱动器319改变长轴方向的振动幅度a；通过幅度控制PID314输出控制力C_q，通过差分驱动器320改变正交方向的振动幅度q；通过相位控制PID316输出控制力C_r，通过差分驱动器317改变谐振子的振动频率ω。

首先，对于所述半球谐振陀螺，当有角速度输入时，谐振子301受迫振动产生的四波腹驻波位置发生变化，8个检测电极302表面与谐振子表面形成的电容容值随之改变，其中相隔90゜的正负电极上的电容变化量大小相同，符号相反。±x，±y检测电极的电容变化量经过x路电容/电压转换模块304和y路电容/电压转换模块305转换后转换为电压信号，后经x路信号调理模块306、差分放大模块308和y路信号调理模块307、差分放大模块309即可得到公式(3)中所述的x和y信号，x和y信号进入信号解调模块310后，可得到含有长轴波长a，正交轴波长q，振动频率ω和驻波进动角θ相关的变量，再经信号解算模块311解算后得可到a，q，ω和θ、全角模式下的角度变化量

或力平衡模式下的角速度Ω，以及角速度变化量

a，q和ω输入到幅度控制PID 314，正交控制PID313，相位控制PID 316(频率跟踪PID)，和对应模块的参考值比较后得到控制误差，经过PID控制解算后得到控制力C_a，C_q和C_r，这些控制力经过差分驱动器320，319和317后，通过激励电极303自动调整相应控制力的大小来补偿扰动，使谐振子振动逼近理想状态。经过信号解算模块311解算后还得到进动角θ，角速度Ω角以及速度变化量

综合这些信号，可以依据本发明所述的一种半球谐振陀螺的工作模式切换控制方法来控制模式控制开关312，实现半球谐振陀螺的双模式控制。

若选择力平衡模式，则将解算的进动角θ与参考进动角比较得到控制力C_p的大小，再经过差分驱动器318，通过激励电极保证谐振子的驻波位置不动，此时的控制力C_p大小综合陀螺标度因数即可得到陀螺相对惯性空间的旋转角速度。若选择全角模式，则直接利用得到的进动角θ，结合陀螺的标度因数即可得到陀螺相对惯性空间的单位时间内角度变化量。

为了让本发明的控制系统能够更好的实施本发明的控制方法，本发明还提供了一个优选实施例，一种更为优选的半球谐振陀螺的工作模式切换控制方法流程，如图5所示，所述控制方法包括：

401、对整个控制系统进行初始化；

这个过程中可以初始化各个硬件以及初始化变量，使半球谐振陀螺起振；这里的硬件包括上述实施例所提到的系统中的半球谐振陀螺、各个模块以及PID控制器等；这里的变量包括上述实施例中所计算的各个参量。

402、幅度、正交和频率控制；

本实施例中，采用幅度控制PID、正交控制PID以及相位控制PID的回路自动调整相应控制力的大小来补偿由于加工制造等误差及使用环境变化等造成的谐振子振动幅度、正交振动幅度及频率的扰动，使谐振子振动逼近理想振动模态。

403、全角模式；

由于力平衡模式一般用于载体小角速率条件，而上电后载体运动角速度大小未知，因此初始化控制模式为全角模式，避免所述控制系统在上电后输出幅度限幅或不稳定。

404、计算角速度增量；

在初始时刻，本实施例需要计算全角模式控制下的驻波的进动角及角度变化量。

405、计算角加速度并滤波；

根据角度变换量计算出角加速度

并进行滤波以降低角加速度信号噪声，为载体处于加速状态还是减速状态的判定提供依据。

406、判断角加速度

是否大于0，转至步骤407或者步骤410；

407、若角加速度

则继续判断角速度绝对值|Ω|与第一判断阈值ω₁＝ω_s+ω_th的大小，转至步骤408；

408、若|Ω|<ω_s+ω_th，则进入力反馈模式转至步骤409，否则进入全角模式转至步骤412；

409、计算出力平衡模式下的角速度，并转至步骤413；

410、若角加速度

则继续判断角速度绝对值|Ω|与第二判断阈值ω₂＝ω_s-ω_th的大小，

411、若|Ω|≥ω_s-ω_th，则进入全角模式转至步骤412，否则进入力平衡模式转至步骤409；

412、计算出全角模式下的角速度增量；

413、输出全角模式或者力平衡模式下的角速度；可以输出全角模式下单位时间内角度的变化量或力平衡模式下的角速度。

本实施例将控制方法与控制系统进行了展现，可以看出，本发明可以很好的完成力平衡模式和全角模式混合模式控制下的陀螺角速度的测量。

以上所述的仅是本发明的实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

在本发明的描述中，需要理解的是，术语“同轴”、“底部”、“一端”、“顶部”、“中部”、“另一端”、“上”、“一侧”、“顶部”、“内”、“外”、“前部”、“中央”、“两端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“设置”、“连接”、“固定”、“旋转”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (6)

1.一种半球谐振陀螺的工作模式切换控制方法，其特征在于，所述方法包括：

S1、计算出当前半球谐振陀螺的角加速度并进行滤波；

S2、若角加速度＞0且角速度的绝对值＜第一判断阈值，或者角加速度≤0且角速度的绝对值＜第二判断阈值，则进入力平衡模式；

S3、在力平衡模式下，按照力平衡PID控制器施加控制力C_p，使谐振子的驻波位置不动，实时计算和输出角速度，并返回步骤S1；

S4、若角加速度≤0且角速度的绝对值≥第二判断阈值，或者角加速度＞0且角速度的绝对值≥第一判断阈值，则进入全角模式；

S5、在全角模式下，按照力平衡PID控制器切断力平衡回路所施加的控制力使得C_p＝0，实时计算和输出角速度增量，并返回步骤S1。

2.根据权利要求1所述的一种半球谐振陀螺的工作模式切换控制方法，其特征在于，所述方法还包括通过正交控制PID施加控制力C_a控制谐振子长轴方向的振动幅度；通过幅度控制PID施加控制力C_q控制谐振子正交方向的振动幅度；通过相位控制PID施加控制力C_r控制谐振子的振动频率；从而自动调整相应控制力C_a、C_q和C_r的大小来补偿扰动。

3.根据权利要求1所述的一种半球谐振陀螺的工作模式切换控制方法，其特征在于，所述第一判断阈值的计算公式表示为ω₁＝ω_s+ω_th；其中，ω₁表示第一判断阈值；ω_s表示模式转换阈值；ω_th表示模式切换滞回值。

4.根据权利要求1所述的一种半球谐振陀螺的工作模式切换控制方法，其特征在于，所述第二判断阈值的计算公式表示为ω₂＝ω_s-ω_th；其中，ω₂表示第二判断阈值；ω_s表示模式转换阈值；ω_th表示模式切换滞回值。

5.一种半球谐振陀螺的工作模式切换控制系统，其特征在于，所述控制系统包括由谐振子、检测电极和激励电极构成的半球谐振陀螺；通过电容/电压转换模块将检测电极的电容变换量转换为电压信号；通过信号调理模块、差分放大模块、信号解调模块以及信号解算模块解算后得到力平衡模式下的角速度或者全角模式下的角度变化量和角加速度；根据力平衡模式下的角速度或者全角模式下的角度变化量和角加速度切换模式控制开关，所述模式控制开关断开时所述控制系统处于全角模式；所述模式控制开关闭合时所述控制系统处于力平衡模式，所述模式控制开关连接有力平衡PID控制器，所述力平衡PID控制器输出施加控制力C_p，经过差分驱动器后通过激励电极使得驻波位置不动。

6.根据权利要求5所述的一种半球谐振陀螺的工作模式切换控制系统，其特征在于，所述信号解算模块还解算出长轴方向的振动幅度、正交方向的振动幅度和谐振子的振动频率，通过正交控制PID输出控制力C_a，通过差分驱动器改变长轴方向的振动幅度；通过幅度控制PID输出控制力C_q，通过差分驱动器改变正交方向的振动幅度；通过相位控制PID输出控制力C_r，通过差分驱动器改变谐振子的振动频率。

Images