基于EKF 算法的三轴气浮台高精度姿态确定系统

刁雁华 夏红伟

（1.南京国睿防务系统有限公司 江苏省南京市 210012）

（2.哈尔滨工业大学空间控制与惯性技术研究中心 黑龙江省哈尔滨市 150001）

1 引言

三轴气浮台作为空间飞行器地面仿真系统的核心设备，其姿态确定系统分为两类，一类依托姿态敏感器的信息，另一类主要借助计算机视觉完成姿态的测量。

确定性算法和状态估计算法是姿态确定算法的主要研究方向。确定性算法起源于Wahba 问题，该方法具有实用性好，操作性强，精度不高的特点。状态估计算法基于状态空间表达式，根据姿态描述特征的不同，发展出欧拉角、四元数等改进算法。状态估计法模型建立灵活，是目前的主流方法。

本文就三轴气浮台的姿态确定问题，设计实现姿态确定系统，选定测量元件和定姿方案。研究并提出了一种高精度姿态确定算法，并开展地面全物理仿真试验工作。

2 系统总体设计

三轴气浮台高精度姿态确定系统，包括高精度激光陀螺1 套、激光跟踪仪1 台、T-Probe 智能测头1 个、光电自准直仪2 台、立方棱镜1 个、工业控制计算机2 台。

系统分为台上、台下两个子系统。台上子系统包括安装在台体上的陀螺、T-Probe、立方体棱镜和工业控制计算机1（台上工控机）；台下子系统包括分布在气浮台周围的激光跟踪仪、光电自准直仪和工业控制计算机2（台下工控机），其中两台光电自准直仪90°安装。

在传感器的位置分布方面，需同时满足以下试验条件：

条件1：立方棱镜能够同时进入两台光电自准直仪的视场范围；

条件2：T-Probe 能够进入激光跟踪仪的视场范围；

条件3：激光跟踪仪的视场可覆盖光电自准直仪的视场范围。

光电自准直仪、激光跟踪仪分别搭配立方棱镜、T-Probe，给出台体姿态；台下工控机接收传感器数据并转发给台上；台上工控机同时接收台下数据和陀螺角速度，经算法处理后输出姿态信息，给到姿态控制系统的闭环反馈链路，同时下发给台下子系统；台下工控机完成数据记录，用于事后分析和效能评估。系统工作流程如图 1 所示。

图1： 工作流程与数据流情况

3 姿态描述

3.1 坐标系

根据姿态描述关系与传感器特征，建立地理坐标系、气浮台台体坐标系、陀螺量测坐标系、陀螺立方棱镜坐标系、激光跟踪仪坐标系、T-Probe 坐标系、立方棱镜的标定坐标系及其量测坐标系。

地理坐标系（G 系）视为姿态参考，气浮台台体坐标系（B系）与气浮台本体固联。陀螺量测坐标系（Gyro 系）描述角速度的分布情况，其陀螺棱镜坐标系（P 系）用于辅助标定陀螺的安装矩阵 。激光跟踪仪坐标系（J 系）与T-Probe坐标系（T 系）的相对关系表示姿态变化。立方棱镜的标定坐标系（L 系）和量测坐标系（D 系）分别应用于位置标定和姿态量测阶段。

3.2 姿态参数和运动学方程

3.2.1 欧拉四元数

3.2.2 欧拉四元数的运动学方程

B 系下的角速度为 ，则四元数的运动学方程为：

3.3 姿态敏感器

3.3.1 陀螺

光电自准直仪根据台上立方棱镜反射光线的位置反映姿态变化，用于小角度场景。定义初始时刻的台体坐标系（B系）；当前时刻的台体坐标系（B系）；初始时刻的立方棱镜测量坐标系，（D系）；当前时刻的立方棱镜测量坐标系（D系）。

4 基于EKF的高精度姿态确定算法

4.1 偏差四元数

偏差四元数描述四元数实际值相对预测量的微小转动，其标量部分近似为1，矢量部分体现转动姿态。设q 是量测姿态四元数， 是估计，则偏差四元数q为：

4.2 偏差四元数的运动学方程

4.3 状态方程

表示陀螺的角速度在B 系的投影， 表示台体角速度在B 系的投影，其中b 为陀螺三轴漂移在B 系的投影，n为量测噪声在B 系的投影：

4.4 量测方程

选择B 系相对I 系到偏差四元数的矢量部分Q作为量测值，建立量测方程，其中v是激光跟踪仪或光电自准直仪的量测噪声：

5 姿态确定方案

高精度姿态确定系统采用量测元件联合确定台体姿态信息，细分为三种子方案，具体配置如下：

5.1 方案一：“陀螺+光电自准直仪/激光跟踪仪”联合定姿

场景：台体在光电自准直仪的检测范围内。

台下子系统：光电自准直仪和激光跟踪仪分别确定姿态角，发送给台上子系统。

台上子系统：台下姿态配合陀螺角速度，经EKF 算法输出台体姿态。

5.2 方案二：“陀螺+激光跟踪仪” 联合定姿

场景：台体脱离光电自准直仪的视场范围，并保持在激光跟踪仪的视场范围中。

台下子系统：采集并转发激光跟踪仪输出的台体姿态角度给台上。

台上子系统：接收台下信息，配合陀螺经EKF 算法，输出台体实时姿态角和姿态角速度。

5.3 方案三：陀螺独立定姿

场景：台体脱离激光跟踪仪的视场范围。

台下子系统：激光跟踪仪和光电自准直仪均无法给出姿态数据。

台上子系统：陀螺角速度积分输出姿态角和姿态角速度。

三种子方案联合使用，达到全局定姿的效果，如图 2 所示，方案一优先级最高，后续依次为方案二和方案三。

图2： 联合姿态确定的子方案选择与切换流程图

6 仿真试验

试验设备：光电自准直仪、激光跟踪仪、陀螺。

试验描述：控制气浮台绕B 系z 轴机动。

指令角：[0° 0° 120°]→[0° 0° 170°]→[0° 0° 120°]。

气浮台初始姿态120°，后机动到170°，最后回归120°，试验时长550s。其中0-6s、533-550s 是稳态阶段，7-532s是机动阶段。

方案一姿态确定：0-4.5s 和513.2-550s 时，台上棱镜处于光电自准直仪的视野范围，光电自准直仪输出姿态角与陀螺角速度输出给EKF 算法处理。

方案二姿态确定：气浮台离开光电自准直仪视场，切换到子方案二，对应4.6-197.6s 和317.6-513.1s。EKF 处理激光跟踪仪和陀螺数据后给出台体姿态角。

方案三姿态确定：气浮台离开激光跟踪仪的视场，使用方案三，从198.7 到317.5s，依靠陀螺自主完成姿态解算工作。

姿态全程曲线如图 3 所示，x 轴和y 轴姿态角维持0°附近，z 轴较稳定地实现机动与回归。

图3： 三种定姿方案对应的姿态角曲线

试验结论：机动过程覆盖全部场景，定姿方案自主切换工作，EKF 算法对多数据源融合滤波，实现姿态确定。

7 结束语

文章围绕三轴气浮台高精度姿态确定问题，从系统设计、姿态确定算法、方案配置、地面全物理仿真试验等四个方面研究分析。搭建姿态确定系统，基于EKF 技术，提出适配系统的姿态确定算法，完成多传感器数据融合与复杂场景下的联合定姿，取得了理想的姿态确定效果，验证了系统设计的可行性和算法的有效性，为空间飞行器及其地面仿真系统的高精度姿态确定领域提供了研究思路。