一种云台全姿态计算方法与流程

本发明属于地面云台装置技术领域，尤其涉及一种云台全姿态计算方法。

背景技术：

云台装置是支撑激光照射器的一种地面配套设备，利用自身惯性测量器件、角度传感器以及转位机构等为激光照射器提供俯仰以及方位两个轴向上的角度变化，引导激光照射器对动静态目标实时地进行跟踪照射，保证半主动式激光导引导弹(苗昊春，杨栓虎，《智能化弹药》[m]，国防工业出版社，2014,2)准确命中目标。

云台在进行俯仰以及方位姿态跟踪之前需要进行调平与寻北。调平目的是保持云台初始姿态水平，保证寻北精度，目前多数云台依靠水泡进行调平，操作复杂，效率低下，无法给出实际俯仰角以及横滚角数值，只能根据肉眼观察，调平精度不高。寻北过程中，云台需要利用自身旋转机构进行多位置转位，由于结构变形等因素会导致云台轴向倾斜，即俯仰角和横滚角发生变化，影响寻北精度，而目前多数云台在转位过程中，俯仰角和横滚角无法实时输出，导致无法评估转位过程中结构变形对寻北精度的影响。

技术实现要素：

本发明针对现有云台姿态跟踪方法存在的无法在保证精度的基础上，自主实时计算云台俯仰角、横滚角以及方位角的问题，提出一种云台全姿态计算方法。

为了实现上述目的，本发明采用以下技术方案：

一种云台全姿态计算方法，包括：

步骤1：分别在云台的轴向和横向安装第一加速度计和第二加速度计，其中在轴向与第一加速度计安装面平行的基准面上安装一个单轴光纤陀螺仪，在云台的纵向上安装一个码盘；

步骤2：利用第一加速度计和第二加速度计在时间段内的输出加速度均值与重力加速度的比值实时解算跟踪俯仰角以及横滚角，对云台进行调平；

步骤3：利用云台的转位机构将单轴光纤陀螺仪旋转到间隔90°的四位置，分别采集四位置的陀螺数据并根据四位置计算方位角模型解算初始方位角；

步骤4：在获取初始方位角后，利用码盘相对于初始方位角旋转的角度变化值，根据码盘方位跟踪模型得出云台的真实方位角。

进一步地，所述步骤2包括：

第一加速度计和第二加速度计按照如下方式实时计算跟踪俯仰角以及横滚角：

θ＝arcsin(f(1)/g)

γ＝arcsin(-f(2)/(g*cos(θ)))

其中θ为云台的俯仰角，γ为云台的横滚角，f(1)为第一加速度计每100ms输出的加速度均值，f(2)为第二加速度计每100ms输出的加速度均值，g为重力加速度。

进一步地，所述步骤3包括：

以云台初始轴向指向为位置1，按逆时针方向、间隔为90°旋转三次，分别得出位置2、位置3、位置4，在每个位置上静态采集陀螺数据40s，并通过误差补偿模型对采集的陀螺数据进行补偿，在累计采集时间结束后对其进行均值处理，在第四个位置采集结束后，共获取4组间隔90°的陀螺静态均值数据。

进一步地，所述四位置计算方位角模型为：

其中为初始方位角，p1g、p2g、p3g、p4g分别为位置1、位置2、位置3、位置4处采集的陀螺静态均值数据。

进一步地，所述码盘方位跟踪模型为：

其中为云台的真实方位角，m实为码盘的实时输出值，m0为码盘的起始零位输出值，为初始方位角。

与现有技术相比，本发明具有的有益效果：

本发明通过分别在云台的轴向和横向安装第一加速度计和第二加速度计，其中在轴向与第一加速度计安装面平行的基准面上安装一个单轴光纤陀螺仪，在云台的纵向上安装一个码盘；然后利用第一加速度计和第二加速度计在时间段内的输出加速度均值与重力加速度的比值实时解算跟踪俯仰角以及横滚角，对云台进行调平；再然后利用云台的转位机构将单轴光纤陀螺仪旋转到间隔90°的四位置，分别采集四位置的陀螺数据并根据四位置计算方位角模型解算初始方位角；最后在获取初始方位角后，利用码盘相对于初始方位角旋转的角度变化值，根据码盘方位跟踪模型得出云台的真实方位角。通过上述方式，本发明可自主实时计算云台俯仰角、横滚角以及方位角；且本发明采用惯性器件(包括第一加速度计、第二加速度计、单轴光纤陀螺仪、码盘)自主进行姿态解算，不易受电磁干扰，可靠性高；本发明可根据实时输出的水平姿态角(包括云台的俯仰角和横滚角)快速调平，减少准备时间；本发明可以在云台快速转位寻北时实时监测云台结构变形度(云台结构变形即姿态变化了，亦即俯仰角和横滚角发生了变化)，评估对寻北精度的影响，并通过在线补偿算法实时补偿，提高寻北精度。

附图说明

图1为本发明实施例一种云台全姿态计算方法的基本流程图；

图2为本发明实施例一种云台全姿态计算方法的器件安装示意图；

图3为本发明实施例一种云台全姿态计算方法的云台的实时俯仰角和横滚角解算原理图；

图4为本发明实施例的一种云台全姿态计算方法的云台的实时方位角解算原理图。

具体实施方式

下面结合附图和具体的实施例对本发明做进一步的解释说明：

如图1所示，一种云台全姿态计算方法，适用于需要实时显示俯仰角、横滚角以及方位角的地面云台装置，包括：

步骤s101：分别在云台的轴向(俯仰轴)和横向(横滚轴)安装第一加速度计和第二加速度计，其中在轴向与第一加速度计安装面平行的基准面上安装一个单轴光纤陀螺仪，在云台的纵向上安装一个码盘，器件安装示意图如图2所示；具体地，所述云台具备多位置转位机构。

步骤s102：在云台的俯仰轴和横滚轴变化速率不高的情况下，利用第一加速度计和第二加速度计在时间段内的输出加速度均值与重力加速度的比值实时解算跟踪俯仰角以及横滚角，对云台进行调平。

具体地，如图3中(a)所示，本发明每2.5ms采集一次轴向(俯仰轴)上的加速度(加速度计1)计数值a1，通过误差补偿模型(秦永元，《惯性导航》[m]，科学出版社，2006,5)对其进行补偿；当云台的俯仰轴处于水平时，加速度计1输出加速度为0m/s²，俯仰角为0°，当俯仰轴缓慢抬头或低头时，可以认为加速度计1只敏感重力加速度g的分量，利用加速度计1每100ms输出的加速度均值f(1)与g的三角函数关系，即可实时求解云台的俯仰角：

θ＝arcsin(f(1)/g)

其中θ为云台的俯仰角，f(1)为第一加速度计每100ms输出的加速度均值，g为重力加速度，a1i为第i次采集的加速度1的计数值，i为采集次数，1≤i≤400。

如图3中(b)所示，本发明每2.5ms采集一次横向(横滚轴)上的加速度(加速度2)计数值a2，通过误差补偿模型对其进行补偿；当云台横滚轴处于水平时，加速度计2输出加速度为0m/s²，横滚角为0°，当横滚轴缓慢倾斜时，可以认为加速度计2只敏感重力加速度g的分量，根据惯性导航解算模型(秦永元，《惯性导航》[m]，科学出版社，2006,5)，俯仰轴与横滚轴是相关的，利用加速度计每100ms输出的加速度均值f(2)与g*cos(θ)的三角函数关系，即可实时求解云台的横滚角：

γ＝arcsin(-f(2)/(g*cos(θ)))

其中θ为云台的俯仰角，γ为云台的横滚角，f(2)为第二加速度计每100ms输出的加速度均值，g为重力加速度，a2i为第i次采集的加速度2的计数值，i为采集次数，1≤i≤400。

步骤s103：利用云台的转位机构将单轴光纤陀螺仪旋转到间隔90°的四位置，分别采集四位置的陀螺数据并根据四位置计算方位角模型解算初始方位角。

具体地，如图4所示，在水平姿态角(包括云台的俯仰角和横滚角)计算结束后，以云台初始轴向指向为位置1，按逆时针方向、间隔为90°旋转三次，分别得出位置2、位置3、位置4，在每个位置上静态采集陀螺数据40s，并通过误差补偿模型对采集的陀螺数据进行补偿，在累计采集时间结束后对其进行均值处理，在第四个位置采集结束后，共获取4组间隔90°的陀螺静态均值数据，分别为p1g、p2g、p3g、p4g，然后利用四位置计算方位角模型求得初始方位(北向)角：

其中为初始方位角，p1g、p2g、p3g、p4g分别为位置1、位置2、位置3、位置4处采集的陀螺静态均值数据，wie为地球自转角速度，la为当地纬度，ε0为陀螺零偏，t为采样时间。

步骤s104：在获取初始方位角后，利用码盘相对于初始方位角旋转的角度变化值，根据码盘方位跟踪模型得出云台的真实方位角。

具体地，在获取水平姿态角以及初始方位角后，利用码盘相对于初始方位角旋转的角度变化值，根据码盘方位跟踪模型得出云台的真实方位角，完成云台方位实时跟踪，所述码盘方位跟踪模型为：

其中为云台的真实方位角，m实为码盘的实时输出值，m0为码盘的起始零位输出值，为初始方位角。

综上，本发明采用惯性器件(包括第一加速度计、第二加速度计、单轴光纤陀螺仪、码盘)自主进行姿态解算，不易受电磁干扰，可靠性高；本发明可根据实时输出的水平姿态角(包括云台的俯仰角和横滚角)快速调平，减少准备时间；本发明可以在云台快速转位寻北时实时监测云台结构变形度，评估对寻北精度的影响，并通过在线补偿算法实时补偿，提高寻北精度。

以上所示仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。