技术领域
[0001] 本
发明属于导航
定位技术领域，具体涉及一种姿态更新方法，适用于捷联式惯性
导航系统(SINS)的姿态解算。
背景技术
[0002] 姿态更新是
机器人、无人机、车辆自主驾驶、
飞行器精确制导与控制等领域的核心问题。捷联式惯性导航系统(Strapdown Inertial Navigation System，SINS)利用载体上的惯性仪表(
陀螺仪、
加速度计)测量姿态的角运动和线运动信息，并由导航计算机根据导航
算法对惯性仪表输出值进行实时处理，完成姿态、速度和
位置等导航参数的实时更新。其中，姿态更新是SINS导航算法的关键，也是决定SINS导航
精度的主要因素之一。
[0003] 载体角运动是一个连续过程，而角运动测量(陀螺仪
采样)却是一个离散过程，这就带来了姿态更新过程中的不可交换性误差，即所谓的刚体有限转动的不可交换性误差。不可交换性误差是姿态更新算法中的原理性误差，不可交换性误差的补偿是提高姿态更新算法精度的关键之一。
[0004] 当前，对不可交换性误差补偿的主流方法为等效旋转矢量法及其各类改进算法，该类算法的核心在于基于确定的角运动模型(多项式运动模型、圆锥运动模型)获得一组确定的不可交换误差补偿系数。由于载体角运动具有随机性和不确定性，角运动模型精度直接决定等效旋转矢量算法等姿态更新算法的精度。一般来说，增加陀螺仪采样
频率可构建出的角运动模型精度越高。但当前技术条件下，
采样频率无法做到绝对高。如何在确定的子样数条件下，提高角运动模型精度成为提高不可交换性误差补偿精度的一个潜在选项。
发明内容
[0005] 发明目的：针对
现有技术中的
缺陷，本发明提出一种基于角速度高次多项式的姿态更新方法，可有效补偿刚体转动引起的不可交换性误差，有效提高载体导航精度。
[0006] 技术方案：本发明所述的一种基于角速度高次多项式的姿态更新方法，包括以下步骤：
[0007] 1)基于陀螺连续输出的N个角增量采样和圆锥运动环境下多项式系数在圆锥轴上的数值关系规律建立角速度的N或N+1次多项式拟合模型；
[0008] 2)利用得到的角速度高次拟合多项式系数和等效旋转矢量的微分方程，由泰勒级数展开求解等效旋转矢量；
[0009] 3)根据等效旋转矢量的计算值，以四元数的形式描述姿态更新周期内的载体的姿态变化。
[0010] 进一步地，所述步骤1)包括：
[0011] 11)在姿态更新周期[0 Nh]内，陀螺仪连续输出N个角增量采样记为Δθi(i＝1,2,…N)，其中h为陀螺仪的采样周期，N为子样数；
[0012] 12)根据载体运动角速度的拟合多项式得到圆锥运动环境下多项式系数在圆锥轴上的数值关系规律，其中，载体运动角速度的拟合多项式为：
[0013]
[0014] 其中，为载体多项式拟合角速度，(i+1)a′i+1为(3×1)维的拟合系数矩阵；
[0015] 多项式系数的数值关系规律近似如下：
[0016] a′j+1×a′k+1＝k0a′l+1×a′m+1,j+k＝l+m＝2n+1,n＝1,2,…
[0017] 其中，a′j+1，a′k+1，a′l+1和a′m+1分别为角速度的拟合多项式中tj，tk，tl和tm项的系数，j,k,l,m分别为一非负整数，他们满足j+k＝l+m＝2n+1这种数值关系，k0为一非零常数；
[0018] 13)建立角速度的N或N+1次多项式拟合模型。
[0019] 进一步地，所述步骤12)中得到圆锥运动环境下多项式系数在圆锥轴上的数值关系规律的过程如下：
[0020] 根据载体运动角速度的拟合多项式： k≥N，
[0021] 以及圆锥运动下载体运动角速度ω(t)的表达式：其中，α为圆锥运动的半锥角，ω0为圆锥频率；
[0022] 两式两边同时对时间t求导并在t＝0处取值，得到角速度系数叉乘项间在x轴上满足如下数值关系：
[0023] (a′j+1×a′k+1)x＝k0(a′l+1×a′m+1)x,j+k＝l+m＝2n+1,n＝1,2,…
[0024] 由于圆锥运动的非周期性误差出现在x轴上，y轴和z轴上仅出现周期性误差，因此考虑x轴上角速度系数叉乘项间的数值关系，由上式近似得到多项式系数的数值关系规律为：a′j+1×a′k+1＝k0a′l+1×a′m+1,j+k＝l+m＝2n+1,n＝1,2,…。
[0025] 进一步地，所述步骤13)包括：
[0026] 根据子样数N的取值推导得到相应的多项式系数间的数值关系式，并结合相应的角增量Δθi(i＝1,2,…N)与角速度多项式系数间的数值关系，推导得到相应子样数条件下角速度拟合多项式。
[0027] 进一步地，所述步骤2)包括：
[0028] 等效旋转矢量微分方程具体形式为：
[0029]
[0030] 其中，φ为等效旋转矢量，φ＝|φ|，ω为角速度，×表示叉乘运算，忽略φ的高次项并将上式中的第二项φ用角增量近似，将等效旋转矢量微分方程简化为：
[0031]
[0032] 采用泰勒级数展开法求解等效旋转矢量，等效旋转矢量在时刻Nh处的泰勒级数解如下：
[0033]
[0034] 初始值φ(0)＝0，将角速度的N或N+1次拟合多项式代入上式即可计算φ(Nh)。
[0035] 进一步地，所述步骤3)包括：
[0036] 由等效旋转矢量φ(Nh)构造的姿态变化四元数为：
[0037]
[0038] 根据姿态变化四元数q(Nh)即可完成时间段[0 Nh]内载体的姿态更新。
[0039] 有益效果：本发明与现有技术相比，具有以下有益效果：
[0040] 1)本发明在不改变子样数N的条件下，充分利用陀螺仪测量值，构建角速度高次多项式拟合模型，减小多项式对载体实际角运动的拟合误差；
[0041] 2)在高次多项式系数求解过程中，以圆锥运动作为约束条件，确定多项式系数的数值解析关系，为多项式系数求解提供约束条件，提高陀螺仪测量值的利用效率；
[0042] 3)基于高次多项式和泰勒展开完成等效旋转矢量及其对应的四元数的求解，提高SINS的姿态解算精度，完成载体姿态信息解算。
附图说明
[0043] 图1为根据本发明
实施例的基于角速度高次多项式的姿态更新方法
流程图；
[0044] 图2为圆锥运动环境下，N＝3时本发明与经典多子样算法、优化多子样算法圆锥漂移误差对比图；
[0045] 图3为圆锥运动环境下，N＝4时本发明与经典多子样算法、优化多子样算法圆锥漂移误差对比图；
[0046] 图4为圆锥运动环境下，N＝5时本发明与经典多子样算法、优化多子样算法圆锥漂移误差对比图；
[0047] 图5为圆锥运动环境下，N＝6时本发明与经典多子样算法、优化多子样算法圆锥漂移误差对比图；
[0048] 图6为圆锥运动环境下，α＝90°，N＝4时本发明与经典多子样算法、优化多子样算法非锥轴角度误差对比图；
[0049] 图7为圆锥运动环境下，α＝0.1°，N＝4时本发明与经典多子样算法、优化多子样算法非锥轴角度误差对比图。
具体实施方式
[0050] 下面结合附图对本发明的技术方案作进一步说明。
[0051] 本发明针对传统利用陀螺输出N子样建立的角运动模型难以真实反映载体角运动的问题，在输出子样数N确定的条件下，利用N子样和圆锥运动约束下角速度拟合多项式系数叉乘项间的数值关系构建角速度的N或N+1次多项式拟合模型，然后利用泰勒级数展开和角速度的拟合求解等效旋转矢量及其对应的姿态四元数，完成姿态更新计算。姿态计算过程中，可有效补偿刚体转动引起的不可交换性误差，利用高精度姿态解算结果参与速度解算和位置解算，提高载体导航精度。
[0052] 参照图1，本发明提出的基于角速度高次多项式的姿态更新方法包括以下步骤：
[0053] 步骤S1，基于陀螺连续输出的N个角增量采样和圆锥运动环境下多项式系数在圆锥轴上的数值关系规律建立角速度的N或N+1次多项式拟合模型，N大于等于3。
[0054] 拟合多项式模型具体包括如下步骤：
[0055] 考虑在姿态更新周期[0 Nh](h为陀螺仪的采样周期，N为子样)内，陀螺仪连续输出的N个角增量采样为Δθi(i＝1,2,…N)。在载体运动角速度为多项式形式的假设条件下，记角速度的拟合多项式为：
[0056]
[0057] 其中， 为载体多项式拟合角速度，k为多项式的次数，(i+1)a′i+1为(3×1)维的拟合系数矩阵。
[0058] 在圆锥运动下，载体运动角速度ω(t)可表示为：
[0059]
[0060] 其中，α为圆锥运动的半锥角，ω0为圆锥运动角频率，简称圆锥频率。
[0061] 同时对式(1)和式(2)两边同时对时间t求导并在t＝0处取值可得角速度系数叉乘项间在x轴上满足如下数值关系：
[0062] (a′j+1×a′k+1)x＝k0(a′l+1×a′m+1)x,j+k＝l+m＝2n+1,n＝1,2,… (3)[0063] 其中，k0为一非零常数，j,k,l,m分别为一非负整数，他们满足j+k＝l+m＝2n+1这种数值关系。
[0064] 对于式(3)表述的圆锥运动，非周期性误差出现在x轴上，y轴和z轴上仅出现周期性误差。因此主要考虑x轴上角速度系数叉乘项间的数值关系，由式(3)近似可得：
[0065] a′i+1×a′j+1＝k0a′i+1×a′j+1,j+k＝l+m＝2n+1,n＝1,2,… (4)
[0066] N＝3时，由式(4)得：
[0067]
[0068] 结合式(5)和角增量Δθi(i＝1,2,3)与角速度多项式系数间的数值关系可得，N＝3时，角速度拟合多项式为：
[0069]
[0070] 其中，
[0071]
[0072] 其中，上标+表示矩阵的广义逆，a1，a2，a3通过式(8)在N＝3时求解。
[0073]
[0074] tj＝jh(j＝1,2,…,N)
[0075] 式(8)是根据角增量等于角速度的积分值直接推导而得到。
[0076] N＝4时，由式(4)得：
[0077]
[0078] 结合式(9)和角增量Δθi(i＝1,2,…4)与角速度多项式系数间的数值关系可得，N＝4时，角速度拟合多项式为：
[0079]
[0080] 其中，
[0081]
[0082] 式中，a1，a2，a3，a4通过式(8)在N＝4时求解。
[0083] 类似地可以得到N≥5时，可求解式(1)中角速度的拟合多项式。其中，N为奇数时，可构建角速度的N次拟合多项式，N为偶数时，可构建角速度的N+1次拟合多项式。如N＝5时，由式(4)可得a′1×a′6＝1/5(a′3×a′4)，考虑到角增量Δθi(i＝1,2,…5)与角速度多项式系数间的数值关系可得角速度的5次拟合多项式；N＝6时，由式(4)可得a′2×a′7＝2/7(a′4×a′5)和a′1×a′6＝-1/14(a′4×a′5)，考虑到角增量Δθi(i＝1,2,…5)与角速度多项式系数间的数值关系可得角速度的7次拟合多项式。
[0084] 步骤S2，利用得到的角速度高次拟合多项式系数(i+1)a′i+1和等效旋转矢量的微分方程，由泰勒级数展开求解等效旋转矢量。
[0085] 具体包括：
[0086] 等效旋转矢量微分方程具体形式为：
[0087]
[0088] 其中，φ为等效旋转矢量，φ＝|φ|，ω为角速度，×表示叉乘运算。忽略φ的高次项并将式(12)中的第二项φ用角增量近似，式(12)可简化为：
[0089]
[0090] 采用泰勒级数展开法求解等效旋转矢量，等效旋转矢量在时刻Nh处的泰勒级数解如下：
[0091]
[0092] 初始值φ(0)＝0。将角速度的N或N+1次拟合多项式代入式(14)即可计算φ(Nh)。如N＝3时，角速度拟合多项式如式(6)所示，此时，
[0093]
[0094] 步骤S3，根据等效旋转矢量的计算值，以四元数的形式描述姿态更新周期内的载体的姿态变化。
[0095] 具体包括：
[0096] 由等效旋转矢量φ(Nh)构造的姿态变化四元数为：
[0097]
[0098] 根据姿态变化四元数q(Nh)即可完成时间段[0 Nh]内载体的姿态更新，完成载体姿态信息解算。
[0099] 下面通过一个仿真实验来验证本发明的有益效果。采用Matlab模拟惯性仪表数据，对于姿态更新算法来说，圆锥运动是最恶劣的环境条件，它会诱发数学平台的严重漂移。圆锥运动数学模型载体运动角速度ω(t)为：
[0100]
[0101] 其中，α为圆锥运动的半锥角，ω0为圆锥频率。
[0102] 通过上述仿真数据模拟得到惯导仪表理论数据，惯导对所述仪表实际采集数据进行采样，用于导航解算，采样周期为10ms。
[0103] 仿真的相关参数包括：圆锥频率ω0＝1Hz；半锥角变化范围α＝0.05″～90°；子样数N＝3～6。
[0104] 圆锥误差漂移计算与非锥轴角度误差计算的验证如下：
[0105] 在普通PC机进行算法验证。仿真进行3s，仿真过程过程中，(1)产生仪表数据；(2)根据仪表数据构建角速度的N或N+1次多项式；(3)分别利用本发明所提的方法，经典多子样算法，优化多子样算法进行等效旋转矢量的求解；(4)计算误差旋转矢量，分析圆锥误差漂移与非锥轴角度误差；(5)改变子样数N，重复上述步骤。图2～图5依次示出对应子样数N＝3～6时，本发明所提的方法(New)、经典多子样算法(Classical1)和优化多子样算法(Classical 2)圆锥误差漂移对比。图6和图7分别给出了在α＝90°，N＝4和α＝0.1°，N＝4情况下，本发明所提的方法(New)、经典多子样算法(Classical1)和优化多子样算法(Classical 2)在非圆锥轴上的角度更新误差比较。
[0106] 图2～图5表明，小半锥角时，本发明的方法精度优于经典多子样算法，整体上低于优化多子样算法且随着子样数N增大，在半锥角很小时，本发明的方法精度高于多子样
优化算法。如N＝3时，本发明的方法在半锥角不小于10°时，精度高于多子样优化算法。而在N＝6时，本发明的方法在半锥角不小于10-2°时，精度高于多子样优化算法。大半锥角时，本发明的方法精度优于优化多子样算法，且不低于经典多子样算法。因此可得，与经典、优化多子样算法相比，本发明所提的方法在整个锥角范围内具有一定的整体优势。
[0107] 图6～图7表明，优化多子样算法的误差最大值较本发明的方法误差高一个数量级。这说明，如果载体出现非周期的圆锥运动，本发明的方法的姿态解算误差远小于优化多子样算法。而经典多子样算法误差曲线与本发明的方法误差曲线几乎重合，两者在非圆锥轴上的误差精度相当。
