CN110440779A - 一种高动态mems谐振式陀螺仪力平衡控制方法 - Google Patents

Abstract

本发明公开了一种高动态MEMS谐振式陀螺仪力平衡控制方法，其大致包括如下步骤：首先，由陀螺仪谐振模型得到其增广形式作为MPC控制器的计算模型；然后，在满足控制器平衡力输出约束的条件下，在每个采样时刻实时计算出最优平衡力序列，并取其向量中的第一个值作为下一时刻的控制量。如此循环计算，最终可得到一组最优平衡力序列使得陀螺仪谐振模型的输出趋近于谐振输出给定值，从而抵消科里奥利力，达到力平衡。本发明能够实现MEMS谐振式陀螺仪振幅和频率的精确控制，为陀螺仪提供精准的模态驱动，保证陀螺仪的测量精度和稳定性。同时扩大了陀螺仪动态范围和测量带宽，提高了尺度因子的线性度和抗干扰能力，提高了MEMS谐振式陀螺仪的灵敏度和整体性能。

Description

一种高动态MEMS谐振式陀螺仪力平衡控制方法

技术领域

本发明涉及一种高动态MEMS谐振式陀螺仪力平衡控制方法。

背景技术

微机电系统(Micro-Electro-Mechanical System,MEMS)谐振式陀螺仪具有体积小、功耗低、质量轻、抗过载能力强等优点，广泛应用于汽车电子、消费电子和国防科技等领域。

控制系统是MEMS谐振式陀螺仪的重要组成部分，主要用于控制陀螺仪微机械的运动状态以及对电容检测电路输出的信号进行处理输出角速度信息。一方面，控制系统使陀螺仪始终工作在谐振状态，保证陀螺仪具有较高的灵敏度，另一方面，控制系统对陀螺驱动模态的振幅进行高稳定控制和正交抑制，保证陀螺仪测量稳定性和精度。

目前，MEMS陀螺仪控制系统多采用数字式，但存在系统性能受限、成本较高、开发周期较长等问题。在MEMS谐振式陀螺仪的运行架构中，力平衡控制得到了广泛的应用。现有的力平衡控制方法主要有自适应、鲁棒控制、滑模控制和PID等。其中：

自适应控制、鲁棒控制等方法设计较为复杂，存在严重的可靠性问题；滑膜控制对系统的参数摄动和外部干扰的不变性是以控制量的高频振动换取的，在实际系统中难以适用。

PID控制凭其结构简单、工作性能稳定等优点被广泛应用于MEMS谐振式陀螺仪控制之中，例如：公开号为CN110160514A的中国发明专利申请公开了一种硅MEMS陀螺仪多回路数字化闭环控制装置，该数字化闭环控制装置能够实现硅微陀螺仪实时在线模态自动匹配，同时配合实现闭环检测和正交误差校正；另外，上述数字化闭环控制装置使用FPGA模块实现控制算法，能够有效抑制温度变化，算法复杂度低，调谐精度高。

然而，该数字化闭环控制装置控制算法选用PID控制，具有因积分而导致系统超调量过大的问题，影响控制精度和效果，且带宽较小，灵敏度较低，抗干扰能力较弱。

发明内容

本发明的目的在于提出一种高动态MEMS谐振式陀螺仪力平衡控制方法，以实现MEMS谐振式陀螺仪振动频率和振动幅度的精确控制，扩大陀螺仪动态范围和测量带宽。

本发明为了实现上述目的，采用如下技术方案：

一种高动态MEMS谐振式陀螺仪力平衡控制方法，包括如下步骤：

S1.由陀螺仪谐振模型得到其增广形式作为MPC控制器的计算模型；

S1.1.对陀螺仪谐振模型标准化得到陀螺仪标准状态空间模型，模型的表达式如下：

其中，u(k)为跟踪偏差，u(k)＝f_k-f(k)；

f_k表示谐振输出给定量，即待抵消的科里奥利力；

f(k)为第k时刻MPC控制器输出的平衡力；

x_m(k)、x_m(k+1)分别表示第k时刻、第k+1时刻MPC控制器的状态变量；

A_m表示陀螺仪标准状态空间模型的状态转移矩阵；

B_m表示陀螺仪标准状态空间模型的控制矩阵；

y(k)表示第k时刻MPC控制器的输出、C_m表示陀螺仪标准状态空间模型的输出矩阵；

S1.2.为消除静差，对陀螺仪标准状态空间模型进行增广处理得到陀螺仪谐振模型的增广矩阵形式，将陀螺仪谐振模型的增广矩阵形式作为MPC控制器计算模型；

其中，增量状态空间形式的陀螺仪谐振模型如下：

其中，x(k)、x(k+1)分别表示第k时刻、第k+1时刻增广矩阵的状态变量；

Δx_m(k)、Δx_m(k+1)分别表示第k时刻、第k+1时刻的状态增量；

y(k+1)表示第k+1时刻MPC控制器的输出；

O_m＝[0 0 ··· 0]，C＝[O_m 1]；

其中，O_m的列数与A_m行数相同，Δu(k)表示第k时刻的控制增量；

S2.将测量值和跟踪偏差u(k)代入MPC控制器计算模型进行状态和输出预测；

其中，状态预测序列和输出预测序列的表达式分别如下：

X＝[x(k+1|k) x(k+2|k),…,x(k+N_p|k)^T；

Y＝[y(k+1|k) y(k+2|k),…,y(k+N_p|k)^T；

其中，测量值是指待抵消的科里奥利力f_k；

X为状态预测序列，x(k+i|k)表示在第k时刻预测的k+i时刻的状态，1≤i≤N_p；

Y为输出预测序列，y(k+i|k)表示在第k时刻预测的k+i时刻的输出，1≤j≤N_p；

建立输出预测序列Y、状态变量x(k)以及控制增量序列ΔF之间的联系，其表达式如下：

Y＝Fx(k)+ΦΔF (3)

其中，N_p为预测时域；N_c为控制时域；

ΔF＝[Δf(k) Δf(k+1)，...，Δf(k+N_c-1)]^T；

其中，Δf(k+z)为第k+z时刻的控制增量，0≤z≤N_c-1；

S3.根据力平衡控制的性能指标J求解全局最优平衡力控制增量序列；

力平衡控制的性能指标J的表达式定义为：

其中，r为第k时刻的谐振输出给定值，r^T＝[1 1 … 1]r(k)，r(k)≡0,k＝1,2,3…；

为ΔF的权值，表示对平衡力增量的软约束，

r_w表示软约束的权值，表示N_c×N_c维的单位矩阵；

根据公式(4)求解全局最优平衡力控制增量序列ΔF；

S4.判断全局最优平衡力控制增量序列ΔF是否满足约束条件；

其中，约束条件为MPC控制器输出的平衡力约束，即公式(5)所示：

f^min≤f(k)≤f^max (5)

其中，f^min、f^max表示第k时刻MPC控制器输出的平衡力f(k)的边界约束；

若不满足约束条件，则转到步骤S5；若满足约束条件，则转到步骤S6；

S5.将全局最优平衡力控制增量序列ΔF修正到约束边界上，并转到步骤S6；

S6.取满足约束条件的控制增量序列ΔF中的第一个值Δf(k)作为新的平衡力控制增量；

S7.叠加Δf(k)得到新的平衡力f(k)，其中，

S8.利用新的平衡力f(k)抵消科里奥利力f_k，并判断是否完全抵消：

若未完全抵消科里奥利力f_k，则转到步骤S2；若完全抵消，则表明达到力平衡。

本发明具有如下优点：

如上所述，本发明提供一种高动态MEMS谐振式陀螺仪力平衡控制方法，该方法能够实现MEMS谐振式陀螺仪振幅和频率的精确控制，为陀螺仪提供精准的模态驱动，保证陀螺仪的测量精度和稳定性。同时，还扩大了陀螺仪动态范围和测量带宽，以及提高了尺度因子的线性度和抗干扰能力，从而大大提高了MEMS谐振式陀螺仪的灵敏度和整体性能。另外，相比于传统的PID算法，本发明力平衡控制方法中的MPC控制过程，能够在考虑陀螺仪力平衡控制系统的约束条件下对未来的状态和输出进行主动预测，并使用优化计算的方法得到使得性能指标最优的控制量，其控制质量远高于被动的PID控制方式。

附图说明

图1为本发明实施例中MEMS谐振式陀螺仪数字控制装置的硬件结构框图。

图2为本发明实施例中力平衡陀螺仪数字控制装置的结构示意图。

图3为本发明实施例中力平衡控制回路结构框图。

图4为本发明实施例中高动态MEMS谐振式陀螺仪力平衡控制方法的流程框图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

结合图1所示，本发明实施例述及一种MEMS谐振式陀螺仪数字控制装置，该数字控制装置包括数据转换模块、高速数字模块、实时控制模块和通信模块四部分。

其中，数据转换模块包括数模转换和模数转换，作用有以下两方面：

①是将陀螺仪表头输出的模拟信号转换为数字信号送入主控模块进行数据处理；

②是将数字信号转化为模拟信号用于陀螺仪表头驱动。

高速数字模块采用高速FPGA进行平台开发，利用高速FPGA配置灵活、并行计算能力强、片上资源丰富等优点，利于克服传统FPGA系统性能的限制，其设计模式采用微处理器-可编程数字式的异构式计算架构，以减少硬件消耗并加快开发进度。

由于FPGA对低速且经常变化的逻辑结构编程较为复杂，本装置采用FPGA+CPU的架构模式，即实时控制模块通过PCI-E总线与FPGA交换数据，处理低速信息，加快开发进度。

通信模块采用PCI-E总线模式，加快FPGA与CPU的数据交换，提高数据交换速度。

图2提供了一种力平衡陀螺仪数字控制装置结构示意图。由图2可知，陀螺仪表头结构输出信号经两路系统前端电路放大处理后，经ADC转为数字信号进入数字控制装置。

数字控制装置对两路数字信号分别进行相干解调处理，分别得到同相和正交信号。

其中一路相干解调后得到的同相和正交信号经锁相环、数控振荡器、DAC产生陀螺仪驱动信号，使陀螺仪始终工作在谐振状态；

另一路经相干解调得到的同相和正交信号通过力平衡控制回路、数模转换作用于表头。

图3提供了一种力平衡控制回路，其力平衡控制方法的原理如下：

控制器C为了跟踪给定值r＝0，即使得被控对象输出y→0，在控制器C的作用下会产生迫使被控对象(陀螺仪谐振模型)G的输入为0的控制器输出的平衡力f(k)用于抵消科里奥利力f_k。当y＝r＝0时表示被控对象的输入为0，f(k)＝f_k，即达到力平衡状态。

被控对象输出y代指陀螺仪谐振模型的在相输出I_x和陀螺仪谐振模型的正交相输出Q_x。

相比于传统的开环控制方式，本发明引入闭环控制回路，提高了陀螺仪力平衡控制系统的动态范围、测量带宽、尺度因子的线性度和抗干扰能力。另外，本发明采用的MPC控制算法使用最优计算的方式综合考虑控制系统的约束条件及未来状态和输出的预测，实时求解得到最优控制量，要比直接采用PID算法的控制系统更进一步加强了上述的性能表现。

本发明设计MPC力平衡控制器实现高动态MEMS谐振式陀螺仪力平衡控制，高动态指高带宽，通过闭环控制方式，相比于开环方式能够很大程度的提高信号测量的范围。

其中，MPC的全称为Model Predictive Control，表示模型预测控制，又常称为滚动时域控制(Moving Horizon Control,MHC)、后退时域控制(Receding Horizon Control,RHC)等。

图4为本发明实施例中力平衡控制方法的流程图，其控制原理描述如下：

在当前采样时刻，根据获得的测量信息，即待抵消的科里奥利力，结合陀螺仪力平衡控制系统的约束条件和力平衡控制的性能指标，在线求解一串最优控制增量序列；

取最优控制增量序列的第一个元素作用于被控对象。

在下一采样时刻，用新的测量信息代替原测量信息，重复上述控制过程。

MPC力平衡控制器的设计过程如下：

首先，由陀螺仪谐振模型得到其增广形式作为MPC控制器的计算模型；

然后，在满足控制器平衡力输出约束的条件下，在每个采样时刻实时计算出最优平衡力序列ΔF，并取其向量中的第一个值作为下一时刻的控制量。

如此循环计算，最终可得到一组最优平衡力序列[f(1),f(2),f(3),…f(k)…]使得y→r，从而有f(k)→f_k，从而达到力的平衡条件。

如图4所示，一种高动态MEMS谐振式陀螺仪力平衡控制方法的详细步骤如下：

S1.由陀螺仪谐振模型得到其增广形式作为MPC控制器的计算模型。

S1.1.对陀螺仪谐振模型标准化得到陀螺仪标准状态空间模型，模型的表达式如下：

其中，u(k)为跟踪偏差，u(k)＝f_k-f(k)。

f_k表示谐振输出给定量，即待抵消的科里奥利力。

f(k)为第k时刻MPC控制器输出的平衡力。

x_m(k)、x_m(k+1)分别表示第k时刻、第k+1时刻MPC控制器的状态变量。

A_m表示陀螺仪标准状态空间模型的状态转移矩阵。

B_m表示陀螺仪标准状态空间模型的控制矩阵。

y(k)表示第k时刻MPC控制器的输出、C_m表示陀螺仪标准状态空间模型的输出矩阵。

S1.2.为消除静差，对陀螺仪标准状态空间模型进行增广处理得到陀螺仪谐振模型的增广矩阵形式，将陀螺仪谐振模型的增广矩阵形式作为MPC控制器计算模型。

其中，增量状态空间形式的陀螺仪谐振模型如下：

其中，x(k)、x(k+1)分别表示第k时刻、第k+1时刻增广矩阵的状态变量。

Δx_m(k)、Δx_m(k+1)分别表示第k时刻、第k+1时刻的状态增量。

y(k+1)表示第k+1时刻MPC控制器的输出。

O_m＝[0 0 ··· 0]，C＝[O_m 1]。

其中，O_m的列数与A_m行数相同，Δu(k)表示第k时刻的控制增量。

S2.将测量值和跟踪偏差u(k)代入MPC控制器计算模型进行状态和输出预测。

其中，状态预测序列和输出预测序列的表达式分别如下：

X＝[x(k+1|k) x(k+2|k),…,x(k+N_p|k)^T。

Y＝[y(k+1|k) y(k+2|k),…,y(k+N_p|k)^T。

其中，测量值是指待抵消的科里奥利力f_k。

X为状态预测序列，x(k+i|k)表示在第k时刻预测的k+i时刻的状态，1≤i≤N_p。

Y为输出预测序列，y(k+i|k)表示在第k时刻预测的k+i时刻的输出，1≤j≤N_p。

建立输出预测序列Y、状态变量x(k)以及控制增量序列ΔF之间的联系，其表达式如下：

Y＝Fx(k)+ΦΔF (3)

其中，

N_p为预测时域；N_c为控制时域；一般的N_p≥N_c，对于超过控制时域N_c的控制增量Δu(k+N_c)…Δu(k+N_p-1)均设置为0。

ΔF＝[Δf(k) Δf(k+1)，...，Δf(k+N_c-1)]^T。

其中，Δf(k+z)为第k+z时刻的控制增量，0≤z≤N_c-1。

S3.根据力平衡控制的性能指标J求解全局最优平衡力控制增量序列。

力平衡控制的性能指标J的表达式定义为：

其中，r为第k时刻的谐振输出给定值，r^T＝[1 1 … 1]r(k)，r(k)≡0,k＝1,2,3…。

为ΔF的权值，表示对平衡力增量的软约束，

r_w表示软约束的权值，表示N_c×N_c维的单位矩阵。

根据公式(4)求解全局最优平衡力控制增量序列ΔF。

S4.判断全局最优平衡力控制增量序列ΔF是否满足约束条件。

其中，约束条件为MPC控制器输出的平衡力约束，即公式(5)所示：

f^min≤f(k)≤f^max (5)

其中，f^min、f^max表示第k时刻MPC控制器输出的平衡力f(k)的边界约束。

若不满足约束条件，则转到步骤S5；若满足约束条件，则转到步骤S6。

S5.将全局最优平衡力控制增量序列ΔF修正到约束边界上，并转到步骤S6。

S6.取满足约束条件的控制增量序列ΔF中的第一个值Δf(k)作为新的平衡力控制增量。

S7.叠加Δf(k)得到新的平衡力f(k)，其中，

S8.利用新的平衡力f(k)抵消科里奥利力f_k，并判断是否完全抵消：

若未完全抵消科里奥利力f_k，则转到步骤S2；若完全抵消，则表明达到力平衡。

由上述过程不难看出，MPC控制的滚动时域优化方式能够实时主动预测未来状态和输出的变化情况，经过性能指标J的计算得到满足当前约束条件下的最优控制量，因此，相比于传统的PID控制方式，MPC控制更具有主动性，因而，能达到更好的跟踪效果。MPC控制还具有控制带宽高、频谱平整、反应灵敏、抗干扰能力较强、控制精度较好等优点。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (1)

1.一种高动态MEMS谐振式陀螺仪力平衡控制方法，其特征在于，包括如下步骤：

S1.由陀螺仪谐振模型得到其增广形式作为MPC控制器的计算模型；

S1.1.对陀螺仪谐振模型标准化得到陀螺仪标准状态空间模型，模型的表达式如下：

其中，u(k)为跟踪偏差，u(k)＝f_k-f(k)；

f_k表示谐振输出给定量，即待抵消的科里奥利力；

f(k)为第k时刻MPC控制器输出的平衡力；

x_m(k)、x_m(k+1)分别表示第k时刻、第k+1时刻MPC控制器的状态变量；

A_m表示陀螺仪标准状态空间模型的状态转移矩阵；

B_m表示陀螺仪标准状态空间模型的控制矩阵；

y(k)表示第k时刻MPC控制器的输出、C_m表示陀螺仪标准状态空间模型的输出矩阵；

S1.2.为消除静差，对陀螺仪标准状态空间模型进行增广处理得到陀螺仪谐振模型的增广矩阵形式，将陀螺仪谐振模型的增广矩阵形式作为MPC控制器计算模型；

其中，增量状态空间形式的陀螺仪谐振模型如下：

其中，x(k)、x(k+1)分别表示第k时刻、第k+1时刻增广矩阵的状态变量；

Δx_m(k)、Δx_m(k+1)分别表示第k时刻、第k+1时刻的状态增量；

y(k+1)表示第k+1时刻MPC控制器的输出；

O_m＝[0 0···0]，C＝[O_m 1]；

其中，O_m的列数与A_m行数相同，Δu(k)表示第k时刻的控制增量；

S2.将测量值和跟踪偏差u(k)代入MPC控制器计算模型进行状态和输出预测；

其中，状态预测序列和输出预测序列的表达式分别如下：

X＝[x(k+1|k)x(k+2|k),…,x(k+N_p|k)^T；

Y＝[y(k+1|k)y(k+2|k),…,y(k+N_p|k)^T；

其中，测量值是指待抵消的科里奥利力f_k；

X为状态预测序列，x(k+i|k)表示在第k时刻预测的k+i时刻的状态，1≤i≤N_p；

Y为输出预测序列，y(k+i|k)表示在第k时刻预测的k+i时刻的输出，1≤j≤N_p；

建立输出预测序列Y、状态变量x(k)以及控制增量序列ΔF之间的联系，其表达式如下：

Y＝Fx(k)+ΦΔF (3)

其中，

N_p为预测时域；N_c为控制时域；

ΔF＝[Δf(k) Δf(k+1) ，...，Δf(k+N_c-1)]T；

其中，Δf(k+z)为第k+z时刻的控制增量，0≤z≤N_c-1；

S3.根据力平衡控制的性能指标J求解全局最优平衡力控制增量序列；

力平衡控制的性能指标J的表达式定义为：

其中，r为第k时刻的谐振输出给定值，r^T＝[1 1…1]r(k)，r(k)≡0,k＝1,2,3…；

为ΔF的权值，表示对平衡力增量的软约束，

r_w表示软约束的权值，表示N_c×N_c维的单位矩阵；

根据公式(4)求解全局最优平衡力控制增量序列ΔF；

S4.判断全局最优平衡力控制增量序列ΔF是否满足约束条件；

其中，约束条件为MPC控制器输出的平衡力约束，即公式(5)所示：

f^min≤f(k)≤f^max (5)

其中，f^min、f^max表示第k时刻MPC控制器输出的平衡力f(k)的边界约束；

若不满足约束条件，则转到步骤S5；若满足约束条件，则转到步骤S6；

S5.将全局最优平衡力控制增量序列ΔF修正到约束边界上，并转到步骤S6；

S6.取满足约束条件的控制增量序列ΔF中的第一个值Δf(k)作为新的平衡力控制增量；

S7.叠加Δf(k)得到新的平衡力f(k)，其中，

S8.利用新的平衡力f(k)抵消科里奥利力f_k，并判断是否完全抵消：

若未完全抵消科里奥利力f_k，则转到步骤S2；若完全抵消，则表明达到力平衡。