[发明专利]一种云台控制系统及其控制方法和装置

权利要求书

1.一种云台控制系统的控制方法，所述云台控制系统包括第一角度传感器、第二角度传感器、电机驱动电路和控制器；所述第一角度传感器设置在云台电机轴，用于检测所述云台电机轴的转角；所述第二角度传感器设置在传动机构输出轴，用于检测所述传动机构输出轴的转角；所述控制器分别与所述第一角度传感器、所述第二角度传感器、以及所述电机驱动电路连接；其特征在于，所述方法应用于所述控制器，所述方法包括：

获取第一角度和第二角度，所述第一角度为所述云台电机轴的转角，所述第二角度为所述传动机构输出轴的转角；

根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度；

将所述传动机构输出轴的期望角度减去所述估计角度的差值，作为第一PID控制的输入计算得到所述云台电机的目标转速；

根据所述第一角度，计算得到所述云台电机的实际转速；

将所述云台电机的目标转速减去所述实际转速的差值，作为第二PID控制的输入计算得到所述云台电机的转速控制信号；

将所述转速控制信号发送至所述电机驱动电路，以控制所述云台电机的转速；

所述根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度，具体包括：

根据所述第一角度，以及角度初始估计公式，计算得到所述传动机构输出轴的初始估计角度，所述角度初始估计公式为其中，表示k时刻的初始估计角度，表示k时刻获取的第一角度，a表示比例系数，b表示偏差，ε表示所述第一角度传感器输出数据的系统误差；

根据所述初始估计角度、所述第二角度、以及角度估计公式，计算得到所述传动机构输出轴的估计角度，所述角度估计公式为表示k时刻的估计角度，表示k时刻的第二角度，R表示所述第二角度传感器的量化误差方差，P_k-1为k-1时刻所述传动机构输出轴的角度方差，Q为所述第一角度传感器输出数据的系统误差方差。

2.根据权利要求1所述的方法，其特征在于，所述根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度，具体包括：

从角度初始估计公式集中获取与所述第一角度对应的角度初始估计公式，所述角度初始估计公式集包含多个角度初始估计公式，每个角度初始估计公式分别对应一个第一角度的范围；

根据所述第一角度，以及获取的所述角度初始估计公式，计算得到所述传动机构输出轴的初始估计角度，获取的所述角度初始估计公式为其中，表示k时刻的初始估计角度，表示k时刻获取的第一角度，a_i表示比例系数，b_i表示偏差，ε_i表示所述第一角度传感器输出数据的系统误差；

根据所述初始估计角度、所述第二角度、以及角度估计公式，计算得到所述传动机构输出轴的估计角度，所述角度估计公式为表示k时刻的估计角度，表示k时刻的第二角度，R表示所述第二角度传感器的量化误差方差，P_k-1为k-1时刻所述传动机构输出轴的角度方差，Q_i为所述第一角度传感器输出数据的系统误差方差。