CN105786027A - 云台控制器的控制算法 - Google Patents

Abstract

一种云台控制器的控制算法，属于应用电子设备进行位置识别的方法，第一步：初始化数据；第二步：读取陀螺仪的传感器姿态数据，使用卡尔曼滤波算法算出云台的姿态角；第三步：对比云台的姿态角的前后变化，换算出其磁编码变化角度；如若磁编码变化角度为零，跳转到第二步；否则进行第四步；第四步：将磁编码变化角度换算为三台电机的控制输出量，控制电机动作，改变电机位置；第五步：跳转到第二步，进行循环操作。其发明目的是对三个运动控制单元分别在三个轴上对经卡尔曼滤波的陀螺仪输出数据进行实时的计算，在云台姿态发生变化时驱动PMSM电机，稳定处于现有的姿态。

Description

云台控制器的控制算法

技术领域

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

背景技术

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

发明内容

针对现有技术的缺失和市场的需要，本发明的目的在于提供一种云台控制器的控制算法。使用卡尔曼滤波姿态解算方法以及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。为达到最佳效果，选取如下初始值：

$X\left(k\right)=\left[\begin{array}{cc}1.616& -0.7419\\ 1& 0\end{array}\right]---\left(9\right)$

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

$B=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]---\left(11\right)$

C＝[1,0](12)

二、第三步所述对比云台的姿态角的前后变化，换算出其磁编码变化角度细分为如下过程：

在受到外界抖动、振动等干扰的情况下，是加速度传感器采集的数据失真，造成姿态解算出来的欧拉角错误，只用角度单环情况下，使系统很难稳定运行，因此可以加入角速度作为内环，角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时，回复迅速，三轴云台使用双闭环PID控制，使用角度作为外环，角速度作为内环，进行姿态PID控制，PID控制算法采用位置式数字PID控制：

$u\left(t\right)=k_{p}e\left(t\right)+k_i{\∫}_0^{t}e\left(t\right)dt+k_d\frac{de\left(t\right)}{dt}---\left(13\right)$

式(13)中u(t)为PID输出值，e(t)为期望值与实际值之差，为积分量，为微分量，k_p、k_i、k_d为比例、积分、微分系数。将积分量、微分量离散化得到PID计算公式

$u\left(t\right)=k_{p}e\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}e\left(j\right)T+k_d\frac{e\left(t\right)-e(t-1)}{T}---\left(14\right)$

式(14)中T为更新时间。基于式(14)，姿态PID控制算法

$AngelPIDOut\left(t\right)=k_{p}e\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}e\left(j\right)T+k_d\frac{e\left(t\right)-e(t-1)}{T}---\left(15\right)$

$AngelRatePIDOut\left(t\right)=k_p{e}^{\′}\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}{e}^{\′}\left(j\right)T+k_d\frac{e^{\′}\left(t\right)-e^{\′}(t-1)}{T}---\left(16\right)$

式(15)为角度环PID计算公式，式(16)为角速度环PID计算公式。AngelPIDOut(t)为角度环PID输出，AngelRatePIDOut(t)为角速度环PID输出。

三、所述第四步细分为如下过程：

通过磁编码tle5012b(这个tle5012b要求是一个标准磁编码)计算出电机的实时位置，进而进行位置环控制，通过位置变换的微分运算，通过速度环的输出得到电流的输入与测得的电流值做差运算控制电机的转矩变化，进而改变电机的位置，从而形成一个位置负反馈闭环控制过程。

中央控制单元MCU根据陀螺仪姿态检测对整个系统进行控制和调度，同时对系统各部分的运行状态进行实时的监测和调整。三个运动控制单元分别在三个轴上对经卡尔曼滤波的陀螺仪输出数据进行实时的检测，在云台姿态发生变化时驱动PMSM电机，稳定处于现有的姿态。

三个运动控制单元在系统进入稳定状态后同时开始工作，协同运行，提高了系统的实时性和可靠性。

本发明的有益效果是：利用本算法对三个运动控制单元分别在三个轴上对经卡尔曼滤波的陀螺仪输出数据进行实时的计算，可在云台姿态发生变化时驱动PMSM电机，稳定处于现有的姿态。三个运动控制单元在系统进入稳定状态后同时开始工作，协同运行，提高了系统的实时性和可靠性。

附图说明

图1，是三轴云台系统框架图。

图2，是三轴云台姿态自稳控制原理图。

图3，位置速度控制器方框图。

图4，是软件算法流程图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。

实施例一

本软件控制算法已经在SC1-A手持云台上得到验证，各功能都已经完善开发！在行走过程中使用SC1-A手持云台拍摄视频，拍摄出的视屏与普通手持云台做了对比，效果明显得到提升。

本云台控制器的软件算法，除了可以在手持云台上使用外，另外也可以在机载云台上使用。

Claims (4)

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\left(k\right)=\left[\begin{array}{cc}1.616& -0.7419\\ 1& 0\end{array}\right]---\left(9\right)$

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

$B=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]---\left(11\right)$

C＝[1,0](12)

3.根据权利要求1所述的一种云台控制器的控制算法，其特征在于：第三步所述对比云台的姿态角的前后变化，换算出其磁编码变化角度细分为如下过程：

进行姿态PID控制计算：

$u\left(t\right)=k_{p}e\left(t\right)+k_i{\∫}_0^{t}e\left(t\right)dt+k_d\frac{de\left(t\right)}{dt}---\left(13\right)$

式(13)中u(t)为PID输出值，e(t)为期望值与实际值之差，为积分量，为微分量，k_p、k_i、k_d为比例、积分、微分系数。将积分量、微分量离散化得到PID计算公式

$u\left(t\right)=k_{p}e\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}e\left(j\right)T+k_d\frac{e\left(t\right)-e(t-1)}{T}---\left(14\right)$

式(14)中T为更新时间。基于式(14)，姿态PID控制算法

$AngelPIDOut\left(t\right)=k_{p}e\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}e\left(j\right)T+k_d\frac{e\left(t\right)-e(t-1)}{T}---\left(15\right)$

$AngelRatePIDOut\left(t\right)=k_p{e}^{\′}\left(t\right)+k_i\underset{j=0}{\overset{t}{\Σ}}{e}^{\′}\left(j\right)T+k_d\frac{e^{\′}\left(t\right)-e^{\′}(t-1)}{T}---\left(16\right)$

式(15)为角度环PID计算公式，式(16)为角速度环PID计算公式。

4.根据权利要求1所述的一种云台控制器的控制算法，其特征在于：所述第四步细分为如下过程：

通过磁编码tle5012b(这个tle5012b要求是一个标准磁编码)计算出电机的实时位置，进而进行位置环控制，通过位置变换的微分运算，通过速度环的输出得到电流的输入与测得的电流值做差运算控制电机的转矩变化，进而改变电机的位置，形成一个位置负反馈闭环控制过程。