[发明专利]云台控制器的控制算法

权利要求书

1.一种云台控制器的控制算法，所述云台控制器包括主控MCU、陀螺仪、 三台PMSM电机及其驱动单元，其特征在于包括如下步骤：

第一步：初始化数据；

第二步：读取陀螺仪的传感器姿态数据，依据这些数据使用卡尔曼滤波算法算 出云台的姿态角；

第三步：对比云台的姿态角的前后变化，换算出其磁编码变化角度；如若磁编 码变化角度为零，跳转到第二步；否则进行第四步；

第四步：将磁编码变化角度换算为三台电机的控制输出量，控制电机动作，改 变电机位置；

第五步：跳转到第二步，进行循环操作。

2.根据权利要求1所述的一种云台控制器的控制算法，其特征在于：第二步 中所述使用卡尔曼滤波算法算出云台的姿态角细分为如下过程：

系统的状态方程为：

x(k)＝A(k,k-1)X(k-1)+B(k,k-1)W(k)(1)

观测方程为：

Y(k)＝C(k)X(k)+V(k)(2)

其中，X(k)为k时刻系统的状态向量；A(k,k-1)为k到k-1时刻的状态转移 矩阵；B(k,k-1)为k到k-1时刻的过程噪声驱动矩阵；Y(k)为k时刻系统的输出 向量；C(k)为测量矩阵；W(k)和V(k)分别为过程噪声和测量噪声，并且被假设 为高斯白噪声和随机数噪声。

根据状态方程和测量方程，有如下卡尔曼滤波的五个递推公式：

状态预测：

协方差阵预测：

P(k,k-1)＝A(k,k-1)P(k-1)A^T(k,k-1)+B(k,k-1)Q(k-1)B^T(k,k-1)(4)

滤波增益：

K(k)＝P(k,k-1)C^T(k)(CkP(k,k-1)C^T(k)+R(k))^-1(5)

协方差阵估计：

P(k)＝(I-K(k)C(k))P(k,k-1)(6)

状态估计：

其中，Y(k)是k时刻卡尔曼滤波前的数据，根据上述5个方程，只要给定 初值X₀和P₀，即可根据k时刻的观测值Y(k)计算出k时刻的状态估计k。为达到 最佳效果，选取如下初始值：

X(k)=1.616-0.741910---(9)]]>

W(k)＝[ε_t,0]^T(10)

B=1000---(11)]]>

C＝[1,0](12)