[发明专利]一种高动态MEMS谐振式陀螺仪力平衡控制方法

摘要

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

主权项

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；若完全抵消，则表明达到力平衡。