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

说明书

技术领域

本发明涉及应用电子设备进行位置识别的方法，涉及将现状态的机械参量 转换为电信号的方法，尤指一种云台控制器的控制算法。

背景技术

云台控制器是一个支撑相机的设备，使相机在拍摄过程中处于稳定状态， 进而可以拍摄出稳定的视频画面。从航拍无人机的舵机控制拍摄的抖动画面到 云台控制拍摄出来的稳定画面，从行走中拿手机录像到行走拿手持云台录像， 从斯泰尼康拍摄电影中人物运动到如今的高精度云台系统拍摄电影中人物运 动。这一系列的改变，人们对使用一种简洁、易携方便、物美价廉的移动拍摄 终端产品，拍摄稳定视频有着独特的需求。

发明内容

针对现有技术的缺失和市场的需要，本发明的目的在于提供一种云台控制 器的控制算法。使用卡尔曼滤波姿态解算方法以及FOC电机控制算法，使云台 控制算法得到优化，提高云台产品的控制精度。

本发明解决其技术问题所采用的技术方案是：提供一种云台控制器的控制 算法，所述云台控制器包括主控MCU、陀螺仪、三台PMSM电机及其驱动单元， 其特征在于包括如下步骤：

第一步：初始化数据(包括硬件设备以及软件控制的初始化)；

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

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

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

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

其中：

一、第二步中所述使用卡尔曼滤波算法算出云台的姿态角细分为如下过程：

用卡尔曼滤波利用线性系统状态方程和观测方程，通过观测数据对系统自身状 态进行估计，使估计的均方误差达到最小，

系统的状态方程为：

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。为达到 最佳效果，选取如下初始值：