本发明涉及云台技术领域，特别是一种基于mems陀螺仪组合的稳定云台及其控制方法。
背景技术：
随着微机械传感器(mems)技术的发展，微机械惯性传感器的性能不断提高，应用领域不断扩展，尤其在惯性姿态测量、无人机飞行控制和陀螺稳定云台方面广泛应用。相对于其他原理的惯性传感器，mems惯性传感器的优势在于体积小、功耗低、成本低，缺点在于精度低、温漂大、噪声高，对于高性能的惯性姿态测量和姿态稳定应用中，常常无法满足性能要求。
技术实现要素：
为解决现有技术中存在的问题，本发明提供了一种基于mems陀螺仪组合的稳定云台及其控制方法，该稳定云台采用多只不同类型陀螺仪传感器构建组合式传感器单元的方法，结合合理的控制架构和流程，发挥不同种类mems陀螺仪传感器的优势，显著提高惯性测量反馈和稳定控制性能。
本发明采用的技术方案如下：
一种基于mems陀螺仪组合的稳定云台，包括云台主体和设于云台上的陀螺仪传感器，所述陀螺仪传感器包括：
a类mems陀螺仪传感器，用于角速度控制；
b类陀螺仪传感器，用于姿态解算和角度控制。
传感器单元由至少两种型号的陀螺仪传感器组成，用于稳定负载信息测量的陀螺仪传感器a类mems陀螺仪传感器和b类陀螺仪传感器，每种型号的传感器均至少包含三轴角速度数据，a类mems陀螺仪传感器具有较高的带宽、较低的噪声，b类陀螺仪传感器具有较低的温度漂移和随机漂移。
一种基于mems陀螺仪组合的稳定云台的控制方法，包括如下步骤：
a、校正云台上的a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据；
b、对校正后的a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据进行数据融合；
c、对三轴加速度数据进行姿态解算，得到当前的三轴姿态角度数据；
d、执行角度环外环控制反馈算法得到输出量，其中，反馈量为三轴姿态角度数据，目标量为姿态角度控制目标数据；
e、对三轴姿态角度数据和校正后的a类mems陀螺仪传感器的输出数据进行三轴角速度解耦，得到输出量；
f、执行角速度环内环控制反馈算法得到输出量，并根据该输出量控制伺服电机驱动输出,其中，反馈量为三轴角速度解耦的输出量，目标量为角度环外环控制反馈算法得到输出量。
a类mems陀螺仪传感器的数据用作角速度控制，b类陀螺仪传感器用作姿态解算和角度控制，使用a类mems陀螺仪传感器的角速度数据作为控制反馈的直接输入，使用b类陀螺仪传感器的角速度数据对a类mems陀螺仪传感器的数据进行温漂校正，输出的数据融合了两种传感器的优势，能够显著提高，稳定云台的控制性能，a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据校正后，一起进行“数据融合”，结合了a类mems陀螺仪传感器的瞬态特性和b类陀螺仪传感器的缓慢漂移特性的优点，并将融合结果与三轴加速度数据一起输入到“姿态解算”模块，解算出当前的三轴姿态角度数据，作为“角度环控制”的反馈量运行外环控制算法，“角度环控制”的输出量作为“角速度环控制”的控制目标输入，角速度环控制的输出量作为稳定云台的电机驱动输出，提供给电机驱动部分，两个控制环均可使用通用的反馈控制算法。
在上述步骤中，所述三轴角速度解耦是将陀螺仪的角速度通过解耦矩阵获得各电机运动轴的角速度，解耦算法如下：
令陀螺仪角速度向量为
令三轴解耦矩阵为
其中，φ、θ、ψ分别是俯仰角、横滚角、和航向角；
令三轴电机运动轴角速度矢量为
在上述步骤中，所述姿态解算的步骤如下：
步骤一：陀螺仪积分
陀螺积分项gyrobinteg包含陀螺角速度积分、修正量p和修正量i，
其中，dt是姿态更新周期，t表示本时刻，t-1表示上一时刻，ωbbx、ωbby、ωbbz是陀螺仪角速度，pωx、pωy、pωz是陀螺修正量，△φ、△θ、△ψ是更新周期内的角度增量。
步骤二：姿态矩阵更新
姿态矩阵更新使用方向余弦矩阵算法，利用陀螺积分项gyrobinteg更新姿态矩阵
令：
cosφ＝cos(△φ)
sinφ＝sin(△φ)
cosθ＝cos(△θ)
sinθ＝sin(△θ)
cosψ＝cos(△ψ)
sinψ＝sin(△ψ)
得：
步骤三：姿态融合
姿态融合融合过程本质是一个反馈控制过程，用加计矢量和陀螺矢量的叉积得到修正项p和修正量i，这两个修正量在陀螺积分过程修正陀螺积分值，进而更新姿态矩阵，具体过程如下
1)令
2)令读取三轴重力加速度构成的矩阵为yraw,即
3)对yraw矢量进行归一化处理得
4)求解x、y矢量叉乘，得到向量误差矢量
计算陀螺仪误差修正量p和修正量i，
其中p、i为补偿系数。
步骤四：求解欧拉角
求解欧拉角的过程是利用姿态矩阵
其中φ、θ、ψ分别是俯仰角、横滚角、和航向角。
优选地，所述步骤a还包括以下步骤：
根据预先存储的不同温度条件下的零点偏移数据，拟合出当前温度条件下的偏移量，并根据该偏移量对a类mems陀螺仪传感器和b类陀螺仪传感器的角速度输出数据进行校正。
优选地，所述步骤b包括如下步骤：
b1、对a类mems陀螺仪传感器和b类陀螺仪传感器的角速度数据进行积分，得到角度偏移量。
b2、比较a类mems陀螺仪传感器和b类陀螺仪传感器的角度偏移量，并根据比较结果微调a类mems陀螺仪传感器的校正量，重新进入步骤b1；
数据融合是关键步骤，利用b类陀螺仪传感器温度漂移和随机漂移小的优势，补偿a类陀螺仪在这方面的缺陷，同时保持a类陀螺仪的低噪声、高刷新率的优势。
优选地，所述外环控制算法和内环控制算法为pid控制算法或改良型pid控制算法。
外环控制算法和内环控制算法选用pid，使控制精度更高。
优选地，还包括以下步骤：
从所有mems陀螺仪传感器中优选出数据更新率最高、带宽最高、噪声最低的传感器作为a类mems陀螺仪传感器；
从所有的陀螺仪传感器中优选出温度漂移和随机漂移最低的传感器作为b类陀螺仪传感器。
通过本发明公开的控制方法，使用上述两类陀螺仪传感器构建的陀螺稳定云台，在稳定精度、动态响应和漂移游走等综合指标上，超过了仅使用一类陀螺仪传感器的云台性能，使用两类传感器的云台成本增加极小，但是综合性能大幅度提升，显著提高了整体产品的性价比和竞争力。
优选地，所述b类陀螺仪传感器是mems陀螺仪传感器、光纤陀螺仪传感器和激光陀螺仪传感器的任意一种。
b类陀螺仪传感器的使用不一定要使用mems陀螺仪传感器，也可以使用光纤陀螺仪传感器或激光陀螺仪传感器。
本发明的有益效果如下：
1、采用了三轴陀螺传感器mems陀螺仪传感器组成，每种型号的传感器均至少包含三轴角速度数据，a类mems陀螺仪传感器具有较高的带宽、较低的噪声，具有较低的温度漂移，a类mems陀螺仪传感器的数据用作角速度控制，b类陀螺仪传感器用作姿态解算和角度控制。
2、角度环控制和所述角速度环控制均是pid控制，pid控制精确。
3、增加了数据融合，利用b类陀螺仪传感器温度漂移和随机漂移小的优势，补偿a类陀螺仪在这方面的缺陷，同时保持a类陀螺仪的低噪声、高刷新率的优势。
附图说明
图1为本发明基于mems陀螺仪组合的稳定云台的控制方法的结构图。
具体实施方式
下面结合附图对本发明的实施例进行详细说明。
实施方案1
一种基于mems陀螺仪组合的稳定云台，包括云台主体和设于云台上的陀螺仪传感器，所述陀螺仪传感器包括：
a类mems陀螺仪传感器，用于角速度控制；
b类陀螺仪传感器，用于姿态解算和角度控制。
本实施方案，传感器单元由至少两种型号的mems陀螺仪传感器组成，每种型号的传感器均至少包含三轴角速度数据，a类mems陀螺仪传感器具有较高的带宽、较低的噪声，b类陀螺仪传感器具有较低的温度漂移，a类mems陀螺仪传感器的数据用作角速度控制，b类陀螺仪传感器用作姿态解算和角度控制，使用a类mems陀螺仪传感器的角速度数据作为控制反馈的直接输入，使用b类陀螺仪传感器的角速度数据对a类mems陀螺仪传感器的数据进行温漂校正，输出的数据融合了两种传感器的优势，能够显著提高，稳定云台的控制性能。
实施方案2
如图1所示，如权利要求1所述的基于mems陀螺仪组合的稳定云台的控制方法，包括如下步骤：
a、校正云台上的a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据；
b、对校正后的a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据进行数据融合；
c、对三轴加速度数据进行姿态解算，得到当前的三轴姿态角度数据；
d、执行角度环外环控制反馈算法得到输出量，其中，反馈量为三轴姿态角度数据，目标量为姿态角度控制目标数据；
e、对三轴姿态角度数据和校正后的a类mems陀螺仪传感器的输出数据进行三轴角速度解耦，得到输出量；
f、执行角速度环内环控制反馈算法得到输出量，并根据该输出量控制伺服电机驱动输出,其中，反馈量为三轴角速度解耦的输出量，目标量为角度环外环控制反馈算法得到输出量。
a类mems陀螺仪传感器的数据用作角速度控制，b类陀螺仪传感器用作姿态解算和角度控制，使用a类mems陀螺仪传感器的角速度数据作为控制反馈的直接输入，使用b类陀螺仪传感器的角速度数据对a类mems陀螺仪传感器的数据进行温漂校正，输出的数据融合了两种传感器的优势，能够显著提高，稳定云台的控制性能，a类mems陀螺仪传感器和b类陀螺仪传感器的输出数据校正后，一起进行“数据融合”，结合了a类mems陀螺仪传感器的瞬态特性和b类陀螺仪传感器的缓慢漂移特性的优点，并将融合结果与三轴加速度数据一起输入到“姿态解算”模块，解算出当前的三轴姿态角度数据，作为“角度环控制”的反馈量运行外环控制算法，“角度环控制”的输出量作为“角速度环控制”的控制目标输入，角速度环控制的输出量作为稳定云台的电机驱动输出，提供给电机驱动部分，两个控制环均可使用通用的反馈控制算法。
在上述步骤中，所述三轴角速度解耦是将陀螺仪的角速度通过解耦矩阵获得各电机运动轴的角速度，解耦算法如下：
令陀螺仪角速度向量为
令三轴解耦矩阵为
其中，φ、θ、ψ分别是俯仰角、横滚角、和航向角；
令三轴电机运动轴角速度矢量为
在上述步骤中，所述姿态解算的步骤如下：
步骤一：陀螺仪积分
陀螺积分项gyrobinteg包含陀螺角速度积分、修正量p和修正量i，
其中，dt是姿态更新周期，t表示本时刻，t-1表示上一时刻，ωbbx、ωbby、ωbbz是陀螺仪角速度，pωx、pωy、pωz是陀螺修正量，△φ、△θ、△ψ是更新周期内的角度增量。
步骤二：姿态矩阵更新
姿态矩阵更新使用方向余弦矩阵算法，利用陀螺积分项gyrobinteg更新姿态矩阵
令：
cosφ＝cos(△φ)
sinφ＝sin(△φ)
cosθ＝cos(△θ)
sinθ＝sin(△θ)
cosψ＝cos(△ψ)
sinψ＝sin(△ψ)
得：
步骤三：姿态融合
姿态融合融合过程本质是一个反馈控制过程，用加计矢量和陀螺矢量的叉积得到修正项p和修正量i，这两个修正量在陀螺积分过程修正陀螺积分值，进而更新姿态矩阵，具体过程如下
1)令
2)令读取三轴重力加速度构成的矩阵为yraw,即
3)对yraw矢量进行归一化处理得
4)求解x、y矢量叉乘，得到向量误差矢量
计算陀螺仪误差修正量p和修正量i，
其中p、i为补偿系数。
步骤四：求解欧拉角
求解欧拉角的过程是利用姿态矩阵
其中φ、θ、ψ分别是俯仰角、横滚角、和航向角。
实施方案3
在实施方案2的基础上，所述步骤a还包括以下步骤：
根据预先存储的不同温度条件下的零点偏移数据，拟合出当前温度条件下的偏移量，并根据该偏移量对a类mems陀螺仪传感器和b类陀螺仪传感器的角速度输出数据进行校正。
实施方案4
在实施方案2的基础上，所述步骤b包括如下步骤：
b1、对a类mems陀螺仪传感器和b类陀螺仪传感器的角速度数据进行积分，得到角度偏移量。
b2、比较a类mems陀螺仪传感器和b类陀螺仪传感器的角度偏移量，并根据比较结果微调a类mems陀螺仪传感器的校正量，重新进入步骤b1；
数据融合是关键步骤，利用b类陀螺仪传感器温度漂移和随机漂移小的优势，补偿a类陀螺仪在这方面的缺陷，同时保持a类陀螺仪的低噪声、高刷新率的优势。
实施方案5
在实施方案2的基础上，所述外环控制算法和内环控制算法为pid控制算法或改良型pid控制算法。
外环控制算法和内环控制算法选用pid，使控制精度更高。
实施方案6
在实施方案2的基础上，还包括以下步骤：
从所有的mems陀螺仪传感器中优选出数据更新率最高、带宽最高、噪声最低的传感器作为a类mems陀螺仪传感器；
从所有的陀螺仪传感器中优选出温度漂移和随机漂移最低的传感器作为b类陀螺仪传感器。
通过本发明公开的控制方法，使用上述两类陀螺仪传感器构建的陀螺稳定云台，在稳定精度、动态响应和漂移游走等综合指标上，超过了仅使用一类陀螺仪传感器的云台性能，使用两类传感器的云台成本增加极小，但是综合性能大幅度提升，显著提高了整体产品的性价比和竞争力。
实施方案7
在实施方案6的基础上，所述b类陀螺仪传感器是mems陀螺仪传感器、光纤陀螺仪传感器和激光陀螺仪传感器的任意一种。
b类陀螺仪传感器的使用不一定要使用mems陀螺仪传感器，也可以使用光纤陀螺仪传感器或激光陀螺仪传感器。
以上所述实施例仅表达了本发明的具体实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。