旋转矢量多迭代捷联姿态解算误差补偿算法

王　真，高凤岐，高　敏，高伟伟

（军械工程学院，河北　石家庄　050003）

旋转矢量多迭代捷联姿态解算误差补偿算法

王真，高凤岐，高敏，高伟伟

（军械工程学院，河北石家庄050003）

为克服高动态条件下的捷联姿态解算存在不可交换性误差的问题，达到进一步增强捷联姿态误差抑制效果的目的，基于角速率的输出提出了等效旋转矢量三子样二次迭代优化算法，推导对应的圆锥补偿算法方程及其表达式。分别在不同圆锥运动频率情况下和不同姿态更新频率情况下，展开仿真验证算法的漂移误差和俯仰角误差，以传统的四元数法、三子样算法为对照，分析仿真数据曲线，得出本改进算法在精度和稳定性方面均有较大提高。在单轴速率转台上进行光纤陀螺的实测验证中，通过调整圆锥运动半偏角和频率，测量获取光纤陀螺惯组输出情况，结果表明：该算法在高动态条件下受圆锥半角、圆锥运动频率的影响较小，性能更加优越。

捷联姿态测量；高动态；圆锥误差补偿；等效旋转矢量

0　引言

捷联惯性导航系统一般与载体固联，直接感测载体的运动［1］，姿态实时计算是捷联惯导的关键技术［2］。在导弹等高过载、高频姿态变化的高动态环境下，传感器采集的离散点信息较难反映真实的载体空间转动次序，圆锥运动时尤为明显，故也称圆锥误差。圆锥误差是姿态解算的重难点问题，方向余弦法、四元数法［3］、三子样算法［4］等传统圆锥补偿算法会引起较大的不可交换性误差［5］。作为高动态情形下的误差抑制重难点，圆锥误差的有效抑制对导弹等载体的姿态解算准确度提高有显著意义。

陈建锋等［6］提出一种角速率输入下的二次优化补偿算法，汤传业等［7］引入最小二乘方法进行圆锥误差补偿优化，Song等［8］进一步讨论了传统的圆锥误差补偿算法在机动条件下的实用性问题，黄磊等［9］提出了基于高阶补偿模型的新圆锥算法。

本文针对高动态圆锥误差补偿的难题，以等效旋转矢量三子样算法为基础，基于增加旋转矢量迭代次数提出了一种基于角速率输出的二次迭代改进算法，并优化了旋转矢量计算系数。通过上述算法改进，减小姿态更新过程中的计算量，有利于提高姿态解算准确度，尤其是在高动态条件下提高算法性能。

1　改进型算法的提出

基于陀螺角增量输出是传统的等效旋转矢量法的研究前提，这种方法需要先转换角增量信息为角速率信息，再计算等效旋转矢量，这无疑增加了算法计算量且引入误差。直接针对基于角速率输出的算法研究并不多，本文旨在破除转换环节，直接面向角速率输出，从而提升算法性能和解算准确度。

1.1角速率输出的等效旋转矢量推导

下面推导针对角速率输出的等效旋转矢量，其过程如下：

记tk是第k次采样时刻，设h为姿态更新的周期，且h=tk-tk-1，在姿态更新周期内，记等效旋转矢量为Φ（h），在h=0处用泰勒级数法将Φ（h）展开：

Φ（0）显然为0。ω是载体系相对于参考系的转动角速率，运用三次型抛物线对其进行拟合，过程如下：

令在tk-1、tk-1+h/3、tk-1+2h/3、tk时刻计算出的角速率分别对应为ω1、ω2、ω3、ω4，将其代入上面的公式，得到四元数q（h）：

由此可推得ω（tk-1+），Δθ（）在=0处及各阶导数为

将上式代入Φ（h）在h=0处的泰勒展开式，由角速率系数与角速率的关系，得到基于角速率输出的等效旋转矢量估计值为

上式是对角速率进行三次曲线拟合后得到的，于是称其为基于角速率输出的等效旋转矢量三子样算法。得到等效旋转矢量Φ后，可计算出姿态更新周期内的变化四元数q（h），表示如下：

在下个姿态更新周期的初始时刻，其四元数Q（t+h）可算出，并有如下表示（⊗为四元数乘）：

在姿态更新周期内，四元数更新由此实现。选取在平衡位置附近旋转的圆锥运动，其等效旋转矢量：

式中：α——圆锥角；

Ω——角频率。

在上面描述的运动情况下，其四元数为

根据式（11），得到：

那么，相对应的角速率ω（t）可表示为

若姿态更新频率较大时，sin（Φ/2）≈Φ/2，近似简化后的等效旋转矢量：

旋转矢量误差Φε=Φ-。若想让Φε最小，需要设置低次幂项为零，从而得出在圆锥运动情况下，在沿着圆锥轴轴向的等效旋转矢量的误差估计值为

1.2等效旋转矢量算法的改进

在姿态更新频率较高的条件下，采用多次迭代办法更新旋转矢量，对抑制圆锥误差有积极影响。按照这一思想，在基于角速率输出情况，推导了等效旋转矢量的三子样二次迭代优化算法。

二次迭代更新时，设h为姿态更新周期，其迭代周期T=h/2，［tk-1，tk-1+T］和［tk-1+T，tk-1+2T］则分别为两段迭代周期。在迭代周期［tk-1，tk-1+T］内，相对参考系下的载体系角速率可表示为［10］

用泰勒级数展开Φ（tk-1+T）。

Φ（tk-1）=0，则Φ 在t=tk-1时刻各阶导数皆可知，综上可得：

在［tk-1+T，tk-1+2T］周期内，其表达式亦同理可得。

综合上面的内容，在姿态更新周期内，关于旋转矢量的估计结果表示如下：

旋转矢量增量为

Mij，Nij分别作为角速率叉乘项的待定系数。

对系数优化过程如下：

在典型的圆锥情况时，ωi×ωj仅仅与相对时间间隔h（j-i）有关联。故将上式简化为

在典型圆锥运动条件下，算法的误差漂移［11］：

用泰勒公式将上式展开，可以获到Ωh的各阶系数，令（Ωh）3，（Ωh）5，（Ωh）7的系数为零。算法可表示为如下形式：

其中

表示姿态更新周期内角增量值［12］。

2　仿真实验分析

开展仿真实验的总体思路是将四元数法［13］、三子样算法［14］还有改进算法进行对比，来验证本文改进算法的效果，实验过程中预设圆锥角α=1.5°。

2.1验证算法漂移误差

首先，将圆锥运动的频率固定为4Hz，随着姿态更新频率的变化，观察算法漂移误差（X轴方向分量），如图1所示。然后，将姿态更新频率固定为100Hz，随着圆锥频率的变化，观察算法漂移误差（X轴方向分量）变化情况，如图2所示。

图1　不同姿态更新频率下的算法误差

图2　不同圆锥运动频率下的算法误差

由图知，算法漂移误差与圆锥运动频率呈正比，与姿态更新频率呈反比，改进算法在抑制圆锥运动误差方面性能更佳。

2.2验证俯仰角误差

以俯仰角解算为例［15］，以期望更加直观地分析圆锥运动施加于姿态解算的影响。在较典型圆锥运动条件［16］下，分别采用3种算法计算相对参考系的载体俯仰角。

设α=1.5°，仿真时间t=300s，分别在锥频率4Hz、8 Hz条件下，采用以上算法得到的俯仰角误差结果如图3和图4所示。

2.3分析仿真结果

在设定好的圆锥运动情况下，四元数法解算出的俯仰角误差相较之下明显较大；当圆锥频率比较低时，关于俯仰角解算误差，改进算法优于三子样算法但在精度与稳定性方面相差不是很大，但当圆锥频率明显增大时，改进算法在精度与稳定性方面的优势更加显著。

3　圆锥误差补偿实测数据验证

3.1试验平台及试验条件

为更好地验证本改进型补偿算法在圆锥运动条件下的性能，搭建实测试验平台，其主要组成是被测光纤陀螺和单轴速率转台。

在单轴速率转台上，运用某型光纤陀螺惯组来模拟载体的圆锥运动。用安装偏角模拟半锥角α，圆锥运动频率f由设定速率转台参数来模拟，姿态更新周期h是在姿态解算算法中设定的。具体实验条件如表1所示。

图3　锥频率4Hz下的俯仰角误差

图4　锥频率8Hz下的俯仰角误差

表1　实测的实验条件

3.2实测数据情况

实测试验开展的思路是，调整圆锥运动情况，获取光纤陀螺惯组输出情况，圆锥运动的调整分别由不同α、f来实现。需要指出的是，在实验中没必要对半锥角详细罗列，仅区分较小与较大两大类情况即可，并且分别由as和am表示。

图5　不同模拟圆锥运动下方位角误差

实测3组数据，两两互为对比，具体条件设定如表2所示。

表2　实测的条件设定情况1）

在单轴速率转台上固定惯组后，通过初始精对准得出两个半锥角数值：as=0.9527°，am=3.9318°。以方位角为例，对比3种算法在3组条件下的姿态解算误差情况，结果如图5所示。

3.3实测结果分析

在同一种圆锥运动情况下对比，本改进算法的姿态解算精度比其他两种算法大幅提升，几乎提升了一个数量级。在圆锥半角一样时，圆锥运动频率越大则引起更大的姿态解算误差；在圆锥频率一样时，圆锥半角越大则引起更大的姿态解算误差。本改进型算法相比前两种算法来说，受圆锥半角、圆锥运动频率的影响很小，算法性能更加稳定。

4　结束语

为有针对性地解决高动态条件下传统圆锥补偿算法有不可交换性误差的问题，探讨了基于角速率输出的等效旋转矢量的三子样二次迭代优化算法，推导了相关的圆锥补偿算法方程及表达式。

通过仿真计算和试验验证，对比四元数法和三子样法，证明了本改进算法的优越性。首先，在不同的圆锥频率与不同更新频率情况下，本改进算法的误差精度方面均有较明显提高；其次，本改进算法受圆锥半角、圆锥频率带来的影响更小，高动态下性能更优与稳定性更好。此外，本算法的研究能针对性地解决高动态姿态解算的不可交换性误差补偿的问题，解决了算法复杂度与计算实时性的矛盾，改善整体性能，为下一步继续深入挖掘圆锥误差补偿算法提供有益的理论参考和实验依据。

［1］程承，潘泉，李汉舟.一种新的捷联惯导系统圆锥误差补偿算法研究［J］.弹箭与制导学报，2014，34（1）：1-4.

［2］李海涛，曹咏弘，祖静.等效旋转矢量法在旋转弹姿态解算中的应用［J］.测试技术学报，2011，25（4）：287-291.

［3］张荣辉，贾宏光.基于四元数法的捷联式惯性导航系统的姿态解算［J］.光学精密工程，2008，16（10）：1963-1970.

［4］MILLER R B.A new strapdown attitude algorithm［J］.Journal of Guidance Control and Dynamics，1983，6（4）：287-291.

［5］黄磊，刘建业，曾庆化.基于高阶补偿模型的新圆锥算法［J］.中国惯性技术学报，2013，21（1）：37-41.

［6］陈建锋，陈熙源，祝雪芬.硬件增强角速率圆锥优化算法的姿态解算精度分析及改进［J］.东南大学学报（自然科学版），2012，42（4）：632-636.

［7］汤传业，陈熙源，李建利.一种角速率输入的圆锥算法设计［J］.中国惯性技术学报，2013，21（4）：456-461.

［8］SONG M，WU W Q，PAN X F.Approach to recovering maneuveraccuracyinclassicalconingalgorithms［J］. Journal of Guidance Control and Dynamics，2013，36（6）：1872-1880.

［9］黄磊，刘建业，曾庆化.基于高阶补偿模型的新圆锥算法［J］.中国惯性技术学报，2013，21（1）：37-41.

［10］屈熠，赵忠.增强型等效旋转矢量算法在圆锥运动下的仿真［J］.计算机仿真，2010，27（11）：25-27.

［11］李海涛，曹咏弘，祖静.等效旋转矢量法在旋转弹姿态解算中的应用［J］.测试技术学报，2011，25（4）：287-291.

［12］袁张贤，王鑫伟.斜直圆筒内基于Euler-Rodrigues参数的细长杆螺旋屈曲研究［J］.计算物理，2012，29（4）：549-556.

［13］梁锋，周卫东，马荟.基于四元数的舰船捷联惯导粗对准方法研究［J］.舰船科学技术，2013，35（8）：53-56.

［14］汤传业，陈熙源.一种角速率输入的捷联惯导姿态算法［J］.东南大学学报，2014，44（3）：543-549.

［15］彭孝东，张铁民，李继宇，等.基于传感器校正与融合的农用小型无人机姿态估计算法［J］.自动化学报，2015，41（4）：854-860.

［16］HUANGL，LIU J Y，ZENG Q H.Optimized strapdown coning correctionalgorithm［J］.Transactions of Nanjing University of Aeronautics&Astronautics，2013，30（4）：343-349.

（编辑：李妮）

Research on error compensation improved algorithm of strapdown attitude&heading measurement based on multiple iteration rotation vector

WANG Zhen，GAO Fengqi，GAO Min，GAO Weiwei
（Ordnance Engineering College，Shijiazhuang 050003，China）

To reduce the traditional coning error compensation algorithm noncommutativity errors under high dynamic conditions and to restrain the coning error，proposed the three-sample-andtwo-time iteration algorithm of equivalent rotation vector based on angular rate，and deduced the expression of coning error compensation algorithm.At different coning motion frequency and update frequency，simulation validation showed this algorithm had advantages in accuracy and stability than quaternion and three sub-sample algorithms.Experimental results show that there was smaller influence of coning motion frequency and cone angle on this algorithm，thus this algorithm is better than traditional algorithm under high dynamic conditions.

strapdown attitude&heading measurement；high dynamic；coning error compensation；equivalent rotation vector

A

1674-5124（2016）08-0113-05

10.11857/j.issn.1674-5124.2016.08.023

2015-09-10；

2015-10-11

装备预研共用技术基金项目（9140A05040213JB34069）

王真（1991-），男，河北唐山市人，硕士研究生，专业方向为精密仪器与微系统。