[发明专利]一种高动态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；若完全抵消，则表明达到力平衡。