角速率输入条件下单子样旋转矢量姿态方法
【专利摘要】本发明公开了一种角速率输入条件下单子样旋转矢量姿态方法，该方法基于角速率下多子样旋转矢量算法和角增量下单子样旋转矢量算法，将单子样算法应用到基于角速率的圆锥误差补偿算法中，具体利用当前时刻及前N个时刻的角速率来拟合圆锥误差补偿项，最优补偿项的系数由解线性方程组得到，可以在陀螺仪只提供角速率的情况下，利用角速率输入完成姿态解算并且保持解算频率与采样频率一致。本发明的方法既没有引入由角速率提取角增量带来的误差，又没有降低系统的姿态更新频率。
【专利说明】角速率输入条件下单子样旋转矢量姿态方法
【技术领域】
[0001]本发明属于导航【技术领域】，具体涉及应用于捷联惯性导航系统的姿态解算方法。【背景技术】
[0002]捷联式惯性导航系统(Strap-downInertial Navigation System, SINS)是将加速度计和陀螺仪直接安装在载体上，在计算机中实时计算姿态矩阵，即计算出载体坐标系与导航坐标系之间的关系，从而把载体坐标系的加速度计信息转换为导航坐标系下的信息，然后进行导航计算。
[0003]随着SINS应用越来越广泛，对其精度及动态性能要求也越来越高。姿态更新算法为捷联惯导算法的核心，其中要解决的关键问题是刚体做有限转动时的旋转不可交换性误差。国内外学者提出的多子样旋转矢量算法能够有效地对不可交换性误差进行了补偿。但如果在现有采样频率下直接应用多子样旋转矢量姿态算法，会降低SINS姿态更新频率；若提高采样频率，又会使硬件负担加重以及量化误差更加严重。
[0004]同时，上述算法均建立在陀螺仪输出为角增量的基础上，不能直接应用于输出为角速率的情况。若从角速率中提取出角增量再应用于传统的多子样旋转矢量算法，这样会导致补偿精度下降。有学者提出直接用角速率来计算旋转矢量的算法，避免了提取角增量引入的误差。但其本身仍是一种多子样算法，同样存在姿态更新频率降低的问题。
【发明内容】
[0005]为了同时解决传统多子样旋转矢量姿态算法会降低SINS姿态更新频率和传统角增量算法不能直接应用到角速率输入的问题，提出了一种角速率输入条件下的单子样旋转矢量姿态方法。
[0006]本发明的技术方案为:一种角速率输入条件下的单子样旋转矢量姿态方法，具体包括如下步骤:
[0007]步骤S1.获取设当前k时刻的姿态角为
【权利要求】
1.一种角速率输入条件下的单子样旋转矢量姿态方法，具体包括如下步骤: 步骤S1.获取设当前k时刻的姿态角为attk=[ Θ Y ψ]τ, θ、Y和ψ分别为俯仰角、横滚角和偏航角； 步骤S2、计算旋转矢量ΔΦ; 步骤S3、计算当前k时刻的姿态四元数Q (tk)，具体过程为: 姿态角与坐标转换矩阵Cbn的关系为:
2.根据权利要求1所述的一种角速率输入条件下的单子样旋转矢量姿态方法，其特征在于，步骤S2所述的旋转矢I ΔΦ 11体为利用当前时刻角速率和前四个时刻角速率的五阶单子样算法的旋转矢量。
3.根据权利要求2所述的一种角速率输入条件下的单子样旋转矢量姿态方法，其特征在于，所述的旋转失I卩Δ0的具体计算公式为:
【文档编号】G01C21/16GK103759731SQ201410021105
【公开日】2014年4月30日 申请日期:2014年1月16日 优先权日:2014年1月16日
【发明者】黄盼, 滕云龙, 张晓
申请人:电子科技大学