一种基于大动态环境下不可交换误差抑制方法

1.本发明属于惯性传感器技术的应用领域，特别涉及大动态环境下不可交换误差抑制算法。


背景技术：

2.在惯性导航系统的姿态更新算法中，大动态坏境下载体在不断进行高速转动，这对姿态解算算法的实时性有着较高的要求。因为刚体的转动是不可交换的，由此可知转动不是矢量，在一段时间内对角速度矢量进行积分求解是没有具体意义的，同时大动态坏境下，刚体转动会引入较大的不可交换误差。
3.bortz j和jordon提出采用等效旋转矢量法对大动态环境下的不可交换误差进行补偿，以此达到提高姿态解算精度的目的。周召发等人提出三子样四元数法，将采样数设定为三，先进行旋转失量的求解，再进行四元数解算，得到载体姿态角。基于等效旋转矢量法，很多学者提出了各类不同的扩展改进算法，其中大多以增加子样数来解决大动态环境下的不可交换误差。
4.然而，如今有越来越多的学者通过实验和仿真证明，单纯依靠提升子样数无法从根本上提升大动态环境下姿态解算精度，且随着子样数增加，算法的计算量会大幅提升，这类算法忽略了多子样算法的高阶项，然而在大动态坏境下由高阶项所激励出的误差是不容忽略的。针对以上问题，同时考虑姿态解算算法精度和计算量大小，本文在传统多子样等效旋转矢量算法的基础上提出了一种高阶迭代姿态优化算法。
5.经过检索，申请公开号cn112033437a，一种基于等效旋转矢量算法的捷联惯导系统优化方法，该方法以圆锥运动轨迹作为测试条件，对旋转矢量算法中的系数做优化，根据圆锥运动可得到理论四元数，利用旋转矢量得到计算四元数，对理论四元数与计算四元数作差，得误差四元数，对误差四元数进行泰勒级数展开，通过选择最优化系数，使得误差项为零。根据光纤陀螺输出为角速率的特点，使用角增量和角速率，以角速率为主，其精度高于二子样算法，运算量相当，该方法还能够有效补偿光纤惯导的圆锥误差。在只有角速率输入时，改进算法圆锥补偿精度比传统优化算法要高2个数量级，且计算量比传统优化算法小，该算法适合于由光纤陀螺构成的捷联惯导系统。
6.经过检索，申请公开号cn112033437a，一种基于等效旋转矢量算法的捷联惯导系统优化方法中，该方法应用于器件精度较高的光纤陀螺，目前小型化mems惯性陀螺仪成为行业主流，但其器件精度要低于光纤陀螺，因此对算法有更高的要求。cn112033437a一文中未对大动态坏境进行算法分析，在求解等效旋转矢量时忽略了高阶项，只是对旋转矢量微分方程的前两项采用二次曲线进行拟合，本且，本发明采用双回路对旋转矢量进行更新，能更好平衡算法效率和性能。


技术实现要素：

7.本发明旨在解决以上现有技术的问题。提出了一种基于大动态环境下不可交换误
差抑制方法。本发明的技术方案如下：
8.一种基于大动态环境下不可交换误差抑制方法，其包括以下步骤：以陀螺仪和加速度计作为数据源，以三子样为陀螺仪的一个采样周期，第一个周期内采用等效旋转矢量算法计算求解；下一周期同样以三子样为一个采样周期，采用高阶曲线对其进行拟合，再与上一周期结果迭代求解出高阶迭代解。
9.进一步的，所述以三子样为陀螺仪的一个采样周期，第一个周期内采用等效旋转矢量算法计算求解，具体包括：
10.以陀螺仪和加速度计作为数据源，获取等效旋转矢量，并根据等效旋转矢量得到不可交换误差的修正项，对等效旋转矢量进行泰勒级数展开，再以三子样为陀螺仪采样周期，角速率更新周期划分为等长的三个时间区间，得到三个采样值，以此解算出前一周期的旋转矢量解。
11.进一步的，所述下一周期同样以三子样为一个采样周期，采用高阶曲线对其进行拟合，具体包括：
12.采取四阶曲线进行拟合得到后一周期的拟合曲线，得到多子样待定系数解。
13.进一步的，所述与上一周期结果迭代求解出高阶迭代解，具体包括：
14.利用前一周期的旋转矢量作为观测值，对四阶等效旋转矢量在本周期初始时刻求解出对应的各阶导数，结合旋转矢量微分方程的泰勒级数展开式得到旋转矢量的待定解，再通过两个周期的迭代解求得到本周期的最优解；
15.根据得到的高阶迭代等效旋转矢量解，得到一个姿态跟新周期内的载体坐标系得旋转四元数，重复直至达到姿态信息更新周期后，对姿态信息进行更新。
16.进一步的，所述在一个角速率的更新周期内通过提高子样数实现等效旋转矢量算法的优化，等效旋转矢量微分方程可表示如下：
[0017][0018]
其中表示等效旋转矢量微分、ω角速率采样值、φ表示旋转矢量。
[0019]
在角速率更新周期t＝t
k+1-tk时间段上，通过对载体角速率进行直线、二次抛物线、三次曲线拟合得到其旋转矢量公式，拟合的阶次与其子样数相关，三子样需要采取二次抛物线对其进行拟合：
[0020]
ωb(tk+τ)＝a+2bτ+3cτ2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0021]
其中tk表示当前周期更新的初始时刻，ωb(tk+τ)表示tk+τ时间段上的角速率，a、b、c表示曲线拟合系数，为待求常数。将角速率更新周期t划分为等长的三个时间区间其角增量通式δθi可表示为：
[0022][0023]
i表示对应时间区间上角增量采样次数。
[0024]
将公式(2)带入公式(3)可分别求得各时间区间的角增量如下：
[0025][0026]
对公式(2)进行求导，求得其在周期初始时刻tk下的各阶导数如下：
[0027][0028]
设t＝t
k+1-tk时间段上对于式(1)的旋转矢量微分方程的解为φ(tk+t)，则其泰勒级数展开可表示如下：
[0029][0030]
对公式(1)的前两项进行求导处理，可以得到等效旋转矢量在周期初始时刻tk时对应的各阶导数如下所示：
[0031][0032]
将公式(7)所求解的旋转矢量各阶导数代入旋转矢量微分方程泰勒级数展开式，即公式(6)，整理可得：
[0033][0034]
由公式(4)可以得陀螺输出子样δθ1、δθ2、δθ3和假定参数a、b、c关于更新周t的方程，即将公式(4)代入公式(8)可得：
[0035][0036]
将公式(9)代入公式(8)，并且根据miller得出的三子样基本算法最优解法，对系数进行优化，可以得到三子样等效旋转矢量算法的解：
[0037][0038]
由此可在每个旋转矢量更新周期内实现对旋转矢量的更新，得到前一周期的旋转矢量解。
[0039]
进一步的，对于慢回路计算周期[tk+h,tk+2h]采用高阶曲线进行拟合：
[0040]
ωb(tk+τ)＝a'+2b'τ+3c'τ2+4dτ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0041]
h表示旋转矢量更新周期，采取同步骤(1)中的方法，可将慢回路计算周期[tk+h,tk+2h]角增量δθ4、δθ5、δθ6表示如下：
[0042][0043]
其中a'、b'、c'表示高阶拟合曲线系数，为待求常数。由此得到多子样待定系数解。
[0044]
进一步的，所述得到前一周期的多子样待定系数解为：
[0045][0046]
结合公式(12)和公式(13)由此可得如下关系式：
[0047][0048]
拟合参数与角速率叉乘结果存在以下关系：
[0049][0050]
其中i、j各表示拟合曲线i次项和j次项的参数，且必须满足i≠j，由角速率矢量定义可得：
[0051][0052]
由此可进一步得到各系数间的关系为：
[0053][0054]
结合公式(11)对公式(1)进行求导，得到四阶曲线拟合下等效旋转矢量在周期初始时刻tk时对应的各阶导数如下式：
[0055][0056]
将公式(18)旋转矢量的各阶导数代入旋转矢量微分方程的泰勒级数展开式，结合公式(12)和公式(17)各参数关系表达式可以得到改进算法的最优解如公式(19)所示：
[0057][0058]
由前后两周期的等效旋转矢量解对比分析可知，传统的多子样等效旋转矢量算法复杂度要比改进的算法复杂度低，但高阶项的引入可以更好解决不可交换误差带来的影响，在综合考虑算法精度和算法效率上采用本发明的算法起到对二者更好的平衡作用。
[0059]
本发明的优点及有益效果如下：
[0060]
本发明针对大动态坏境下，传统的等效旋转矢量算法仅仅通过提高子样数无法很好地提高解算精度，忽略了高阶项在大动态环境下会产生较大误差的问题，在此基础上提出了一种大动态环境下不可交换误差抑制算法。该方法以陀螺仪和加速度计作为数据源，以三子样为陀螺仪的一个采样周期，第一个周期内采用等效旋转矢量算法计算求解，下一周期同样以三子样为一个采样周期，采用高阶曲线对其进行拟合，再与上一周期结果迭代求解出高阶迭代解。本发明能更好地抑制不可交换误差，提升imu在大动态坏境下的姿态解
算精度。本发明的主要创新点在于将等效旋转矢量的更新分为快慢两个回路，快回路采用低阶项进行求解，慢回路采用高阶项进行求解，既保证了算法效率，也挺高了算法精度。同时，在传统算法中，等效旋转矢量高阶项往往被忽略，本发明引入高阶项求解等效旋转矢量，能很好解决大动态环境下的不可交换误差问题。相较于传统算法，本发明在姿态解算精度上实现了两个数量级的提升。
附图说明
[0061]
图1是本发明提供优选实施例图1是总体算法框图。
[0062]
图2是传统三子样算法的圆锥环境仿真图。
[0063]
图3是大动态环境下不可交换误差抑制算法的圆锥环境仿真图。
[0064]
图4是传统三子样算法的三轴转台摇摆姿态解算图。
[0065]
图5是大动态环境下不可交换误差抑制算法的三轴转台摇摆姿态解算图。
具体实施方式
[0066]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
[0067]
本发明解决上述技术问题的技术方案是：
[0068]
大动态环境下不可交换误差抑制算法，其特征在于，包括以下步骤：
[0069]
步骤(1)，将等效旋转矢量后两项作为不可交换误差的修正项，对等效旋转矢量进行泰勒级数展开，。再以三子样为陀螺仪采样周期，角速率更新周期划分为等长的三个时间区间，得到三个采样值，以此解算出前一周期的旋转矢量解；
[0070]
步骤(2)，同步骤(1)得到三个采样值，采取四阶曲线进行拟合得到后一周期的拟合曲线，得到多子样待定系数解；
[0071]
步骤(3)，利用前一周期的旋转矢量作为观测值，对四阶等效旋转矢量在本周期初始时刻求解出对应的各阶导数，结合旋转矢量微分方程的泰勒级数展开式得到旋转矢量的待定解，再通过两个周期的迭代解求得到本周期的最优解；
[0072]
步骤(4)，根据得到的高阶迭代等效旋转矢量解，得到一个姿态跟新周期内的载体坐标系得旋转四元数，重复步骤(1)～步骤(3)达到姿态信息更新周期后，对姿态信息进行更新。
[0073]
所述步骤(1)中，在一个角速率的更新周期内通过提高子样数实现等效旋转矢量算法的优化。等效旋转矢量微分方程可表示如下：
[0074][0075]
在角速率更新周期t＝t
k+1-tk时间段上，通过对载体角速率进行直线、二次抛物线、三次曲线拟合得到其旋转矢量公式。拟合的阶次与其子样数相关，三子样需要采取二次抛物线对其进行拟合：
[0076]
ωb(tk+τ)＝a+2bτ+3cτ2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)
[0077]
其中tk表示当前周期更新的初始时刻，ωb表示角速率。将角速率更新周期划分为
等长的三个时间区间由此可知其角增量通式：
[0078][0079]
将公式(40)带入公式(41)可分别求得各时间区间的角增量如下：
[0080][0081]
对公式(40)进行求导，求得其在周期初始时刻tk下的各阶导数如下：
[0082][0083]
设t＝t
k+1-tk时间段上对于式(39)的旋转矢量微分方程的解为φ(tk+t)，则其泰勒级数展开可表示如下：
[0084][0085]
对公式(39)的前两项进行求导处理，可以得到等效旋转矢量在周期初始时刻tk时对应的各阶导数如下所示：
[0086][0087]
将公式(45)所求解的旋转矢量各阶导数代入旋转矢量微分方程泰勒级数展开式，即公式(44)，整理可得：
[0088][0089]
由公式(42)可以得陀螺输出子样δθ1、δθ2、δθ3和假定参数a、b、c关于更新周t的方程，即将公式(42)入公式(46)得：
[0090][0091]
将公式(47)代入公式(46)，并且根据miller得出的三子样基本算法最优解法，对系数进行优化，可以得到三子样等效旋转矢量算法的解：
[0092][0093]
由此可在每个旋转矢量更新周期内实现对旋转矢量的更新，得到前一周期的旋转矢量解。
[0094]
所述步骤(2)中，在步骤(1)的基础上，对于慢回路计算周期[tk+h,tk+2h]采用高阶曲线进行拟合：
[0095]
ωb(tk+τ)＝a'+2b'τ+3c'τ2+4dτ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(49)
[0096]
采取同步骤(1)中的方法，可将慢回路计算周期[tk+h,tk+2h]角增量δθ4、δθ5、δθ6表示如下：
[0097][0098]
由此得到多子样待定系数解。
[0099]
所述步骤(3)中，通过步骤(1)的方法可得到前一周期的多子样待定系数解为：
[0100][0101]
结合公式(50)式(51)此可得如下关系式：
[0102][0103]
拟合参数与角速率叉乘结果存在以下关系：
[0104][0105]
其中i、j各表示拟合曲线i次项和j次项的参数，且必须满足i≠j，由角速率矢量定义可得：
[0106][0107]
由此可进一步得到各系数间的关系为：
[0108][0109]
结合公式(50)对公式(39)进行求导，得到四阶曲线拟合下等效旋转矢量在周期初始时刻tk时对应的各阶导数如下式：
[0110][0111]
将公式(56)旋转矢量的各阶导数代入旋转矢量微分方程的泰勒级数展开式，结合公式(50)和公式(55)各参数关系表达式可以得到改进算法的最优解如公式(57)所示：
[0112][0113]
由前后两周期的等效旋转矢量解对比分析可知，传统的多子样等效旋转矢量算法复杂度要比改进的算法复杂度低，但高阶项的引入可以更好解决不可交换误差带来的影响，在综合考虑算法精度和算法效率上采用本发明的算法起到对二者更好的平衡作用。
[0114]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包
括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0115]
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。