CN107526373A - 一种云台控制系统及其控制方法和装置 - Google Patents

Abstract

本发明公开一种云台控制系统及其控制方法和装置，包括第一角度传感器、第二角度传感器、电机驱动电路和控制器，第一角度传感器检测所述云台电机轴的角度，第二角度传感器检测传动机构输出轴的角度，控制器根据第一角度传感器和第二角度传感器的检测数据，通过电机驱动电路对云台电机的转速进行控制，进而通过传动机构实现实际角度的输出。在云台电机转轴与传动机构输出轴分别安装了角度传感器，通过对角度传感器输出数据的融合处理，实现云台输出转角的准确估计，为云台姿态控制提供准确的转角数据；且，将电机转速信息引入控制环路，有效克服云台的传动机构运动过程中摩擦阻力导致各自由度转速不均匀现象。

Description

一种云台控制系统及其控制方法和装置

技术领域

本发明涉及云台技术领域，更具体地说，涉及一种云台控制系统及其控制方法和装置。

背景技术

云台是一种可以搭载摄像机，且能够在水平和垂直方向转动的承载设备，在摄影、安防、机器人等领域应用广泛。目前，常用的电控云台通常具备水平和垂直两个自由度，云台控制器依据各自由度上安装的角度传感器控制云台电机旋转，实现对云台姿态的控制，进而完成特定区域图像、视频等信息的连续或定点采集。

根据角度传感器安装位置的不同，现有电控云台通常有两种类型。一种是仅在云台电机轴或传动机构输入轴安全角度传感器的云台。这类云台可以实现对云台输出轴转角微小增量的精确控制，但是也存在易受传动机构机械回差影响，导致云台重复定位精度较差的问题。另一种是仅在传动机构输出轴安装角度传感器的云台。这类云台由于角度传感器可精确反应云台输出轴转角，因此具有良好的重复定位精度，但是由于云台电机转动过程中，转速无法实现闭环控制，传动机构摩擦阻力的不均匀会导致云台转动过程中转速出现波动，同时也难以实现对云台输出轴转角微小增量的精确控制。

发明内容

有鉴于此，本发明提出一种云台控制系统及其控制方法和装置，欲实现提高云台控制的精度性和稳定性的目的。

为了实现上述目的，现提出的方案如下：

一种云台控制系统，包括：第一角度传感器、第二角度传感器、电机驱动电路和控制器；

所述第一角度传感器设置在云台电机轴，用于检测所述云台电机轴的转角；

所述第二角度传感器设置在传动机构输出轴，用于检测所述传动机构输出轴的转角；

所述控制器分别与所述第一角度传感器、所述第二角度传感器、以及所述电机驱动电路连接，用于根据所述第一角度传感器和所述第二角度传感器的检测数据，通过所述电机驱动电路对云台电机的转速进行控制。

一种云台控制系统的控制方法，应用于上述控制器，所述方法包括：

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

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

将所述传动机构输出轴的期望角度减去所述估计角度的差值，作为第一PID(proportion、integral、derivative，比例-积分-微分)控制的输入计算得到所述云台电机的目标转速；

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

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

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

一种云台控制系统的控制装置，应用于上述控制器，所述装置包括：

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

传感器数据融合单元，用于根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度；

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

角度微分单元，用于根据所述第一角度，计算得到所述云台电机的实际转速；

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

信号发送单元，用于将所述速度控制信号发送至所述电机驱动电路，以控制所述云台电机的转速。

与现有技术相比，本发明的技术方案具有以下优点：

上述技术方案提供的一种云台控制系统及其控制方法和装置，包括：第一角度传感器、第二角度传感器、电机驱动电路和控制器，第一角度传感器检测所述云台电机轴的角度，第二角度传感器检测传动机构输出轴的角度，控制器根据第一角度传感器和第二角度传感器的检测数据，通过电机驱动电路对云台电机的转速进行控制，进而通过传动机构实现实际角度的输出。在云台电机转轴与传动机构输出轴分别安装了角度传感器，通过对角度传感器输出数据的融合处理，实现云台输出转角的准确估计，为云台姿态控制提供准确的转角数据；且，将电机转速信息引入控制环路，有效克服云台的传动机构运动过程中摩擦阻力导致各自由度转速不均匀现象。有效提升了云台转角控制的精确性和稳定性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种云台控制系统的结构示意图；

图2为本发明实施例提供的一种云台控制系统的控制方法的流程图；

图3为本发明实施例提供的一种将第一角度和第二角度数据融合计算得到估计角度的过程的流程图；

图4为本发明实施例提供的另一种将第一角度和第二角度数据融合计算得到估计角度的过程的流程图；

图5为本发明实施例提供的一种云台控制系统的控制装置的结构示意图；

图6为本发明实施例提供的一种传感器数据融合单元的结构示意图；

图7为本发明实施例提供的另一种传感器数据融合单元的结构示意图。

具体实施方式

针对现有技术方案的不足，本发明通过对云台电机轴和传动机构输出轴安装的角度传感器的输出数据进行融合处理，从而为云台姿态控制提供更加准确可靠的输出转角数据，同时将云台电机速度引入控制环路。从而提升云台控制的精度和稳定性。

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供了一种云台控制系统，参见图1，该系统包括：第一角度传感器1、第二角度传感器2、电机驱动电路3和控制器4；

第一角度传感器1设置在云台电机轴51，用于检测云台电机轴51的转角；

第二角度传感器2设置在传动机构输出轴61，用于检测传动机构输出轴61的转角；

控制器4分别与第一角度传感器1、第二角度传感器2、以及电机驱动电路3连接，用于根据第一角度传感器1和第二角度传感器2的检测数据，通过电机驱动电路3对云台电机5的转速进行控制。

在云台电机轴51与传动机构输出轴61分别安装了角度传感器，通过对角度传感器输出数据的融合处理，实现云台输出转角的准确估计，为云台姿态控制提供准确的转角数据；且将电机转速信息引入控制环路，有效克服云台的传动机构6运动过程中摩擦阻力导致各自由度转速不均匀现象。有效提升了云台转角控制的精确性和稳定性。

本实施例提供了一种云台控制系统的控制方法，应用于控制器4，参见图2，该方法包括：

步骤S11：获取第一角度和第二角度；

第一角度为云台电机轴的转角，第二角度为传动机构输出轴的转角。通过第一角度传感器获取第一角度；通过第二角度传感器获取第二角度。

步骤S12：根据第一角度和第二角度，计算得到传动机构输出轴的估计角度；

通过预先设定的规则，将第一角度和第二角度数据进行融合处理，计算得到传动机构输出轴61的估计角度。

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

利用PID控制算法计算得到云台电机5的目标转速。设传动机构输出轴61的期望角度减去传动机构输出轴61的估计角度的差值为e_k，则云台电机5的 表示是从调整云台输出角度开始至本次计算得到所有差值的和；e_k-1表示前一次计算得到的传动机构输出轴61的期望角度减去传动机构输出轴61的估计角度。

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

对获取的第一角度进行微分处理得到云台电机5的实际转速。

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

利用PID控制算法计算得到云台电机的转速控制信号，通常该速度控制信号为一PWM波形，将该PWM波形输入至电机驱动电路3，实现对云台电机转速的控制。

步骤S16：将所述速度控制信号发送至所述电机驱动电路，以控制所述云台电机的转速。

在上述控制过程中对第一角度传感器和第二角度传感器的输出角度数据进行融合，实现对传动机构输出轴的转角的精确估计。另一方面，将电机转速信息引入控制环路，有效克服传动机构运动过程中摩擦阻力导致各自由度转速不均匀现象。通过上述措施，可有效提升云台转角控制的精确性和稳定性。摩擦阻力导致各自由度转速不均匀现象是指对云台电机施加同样的控制量，云台转动过程中在转角区域速度有时快有时慢，而将速度进行闭环控制后，则可实时检测上述现象，并进行控制，实现转速平稳的目的。

本实施例提供了一种将第一角度和第二角度数据融合计算得到估计角度的过程，预先设定一个角度初始估计公式，参见图3，该过程具体包括：

步骤S21：根据所述第一角度，以及角度初始估计公式，计算得到所述传动机构输出轴的初始估计角度；

预先设定角度初始估计公式其中，表示k时刻的初始估计角度，表示k时刻获取的第一角度，a表示比例系数，b表示偏差，ε表示所述第一角度传感器输出数据的系统误差。

求取角度初始估计公式的过程为，首先控制云台电机旋转，与此同时连续同步采集第一角度传感器和第二角度传感器的输出数据，假设获取得到m组数据，(θ_1,j,θ_2,j)，j＝1,2,…m，m大于等于3；其次，根据获取的m组数据，利用最小二乘算法求得a和b；最后，根据m组数据及与对应关系a和b所确定线性关系之间的偏差计算得到ε和Q。Q为所述第一角度传感器输出数据的系统误差方差。ε和Q的计算过程见下公式：

步骤S23：根据所述初始估计角度、所述第二角度、以及角度估计公式，计算得到所述传动机构输出轴的估计角度；

角度估计公式为 表示k时刻的估计角度，表示k时刻的第二角度，R表示所述第二角度传感器的量化误差方差，P_k-1为k-1时刻所述传动机构输出轴的角度方差，Q为所述第一角度传感器输出数据的系统误差方差。利用误差传递关系得到k时刻传动机构输出轴的角度方差P_k：

P_k＝[1-(a²P_k-1+Q_i)(a²P_k-1+Q_i+R)^-1](a²P_k-1+Q)

量化误差方差为通过对第二角度传感器的输出信息进行数字化过程中的误差，通过实验数据统计，求取数据方差得到。初始时刻传动机构输出轴的角度方差为预设的固定值。

角度估计公式综合考虑了两个传感器的检测数据，如果R相对Q较大，则计算结果就偏向根据第一角度传感器的检测数据得到的初始估计角度；如果Q相对R较大，则计算结果就偏向第二角度传感器的检测数据，从而实现对传动系统误差对电机轴微小变化量和对末端输出的影响的抑制。

本实施例提供了另一种将第一角度和第二角度数据融合计算得到估计角度的过程，预先设定多个角度初始估计公式，参见图4，该过程具体包括：

步骤S31：从角度初始估计公式集中获取与所述第一角度对应的角度初始估计公式；

角度初始估计公式集包含多个角度初始估计公式，每个角度初始估计公式分别对应一个第一角度值的范围。求取多个角度初始估计公式的过程为，首先，将云台电机轴的转动范围等间隔分为n段；其次，控制云台电机旋转，与此同时连续同步采集第一角度传感器和第二角度传感器的输出数据，保证获取得到的数据每段中至少有三组数据，即至少有3n组数据，(θ_1,j,θ_2,j)，j＝1,2,…3n，通过最小二乘法求得每段的a_i和b_i，j＝1,2,…n；最后，根据数据对与对应关系a_i和b_i所确定线性关系之间的偏差即可得到ε_i和Q_i。这样计算得到云台电机轴的n段转动范围对应的各个角度初始估计公式。在获取第一角度后，根据第一角度值所属范围，匹配得到对应的角度初始估计公式。

步骤S32：根据所述第一角度，以及获取的所述角度初始估计公式，计算得到所述传动机构输出轴的初始估计角度；

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

步骤S33：根据所述初始估计角度、所述第二角度、以及角度估计公式，计算得到所述传动机构输出轴的估计角度；

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

通过设定多个角度初始估计公式，可以更加有效获取云台各自由度运动范围内传动机构摩擦阻力和机械回差对输出转角的影响程度，从而进一步保证云台转角控制的精度。

对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。

下述为本发明装置实施例，可以用于执行本发明方法实施例。对于本发明装置实施例中未披露的细节，请参照本发明方法实施例。

本实施例提供了一种云台控制系统的控制装置，应用于控制器4，参见图5，该装置包括：

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

传感器数据融合单元12，用于根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度；

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

角度微分单元14，用于根据所述第一角度，计算得到所述云台电机的实际转速；

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

信号发送单元16，用于将所述速度控制信号发送至所述电机驱动电路，以控制所述云台电机的转速。

对第一角度传感器和第二角度传感器的输出角度数据进行融合，实现对传动机构输出轴的转角的精确估计。另一方面，将电机转速信息引入控制环路，有效克服传动机构运动过程中摩擦阻力导致各自由度转速不均匀现象。通过上述措施，可有效提升云台转角控制的精确性和稳定性。

优选的，传感器数据融合单元12，参见图6，具体包括：

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

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

优选的，传感器数据融合单元12，参见图6，具体包括：

角度估计公式匹配子单元120，用于从角度初始估计公式集中获取与所述第一角度对应的角度初始估计公式，所述角度初始估计公式集包含多个角度初始估计公式，每个角度初始估计公式分别对应一个第一角度值的范围；

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

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

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

对本发明所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种云台控制系统，其特征在于，包括：第一角度传感器、第二角度传感器、电机驱动电路和控制器；

所述第一角度传感器设置在云台电机轴，用于检测所述云台电机轴的转角；

所述第二角度传感器设置在传动机构输出轴，用于检测所述传动机构输出轴的转角；

所述控制器分别与所述第一角度传感器、所述第二角度传感器、以及所述电机驱动电路连接，用于根据所述第一角度传感器和所述第二角度传感器的检测数据，通过所述电机驱动电路对云台电机的转速进行控制。

2.一种云台控制系统的控制方法，其特征在于，应用于权利要求1中所述控制器，所述方法包括：

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

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

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

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

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

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

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

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

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

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

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

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

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

5.一种云台控制系统的控制装置，其特征在于，应用于权利要求1所述控制器，所述装置包括：

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

传感器数据融合单元，用于根据所述第一角度和所述第二角度，计算得到所述传动机构输出轴的估计角度；

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

角度微分单元，用于根据所述第一角度，计算得到所述云台电机的实际转速；

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

信号发送单元，用于将所述速度控制信号发送至所述电机驱动电路，以控制所述云台电机的转速。

6.根据权利要求5所述的装置，其特征在于，所述传感器数据融合单元，具体包括：

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

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

7.根据权利要求5所述的装置，其特征在于，所述传感器数据融合单元，具体包括：

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

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

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