一种基于函数迭代积分的惯性导航解算方法及系统与流程

本发明涉及惯性导航、机器人等技术领域，具体而言，涉及一种基于函数迭代积分的惯性导航解算方法及系统。

背景技术：

三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。惯性导航利用陀螺和加速度计测量角速度和加速度信息，积分获得姿态、速度和位置信息。惯性导航的优点是运动信息丰富，工作过程完全自主，不需要外部信息辅助，特别在卫星导航信号据止或被干扰欺骗等应用场合备受青睐，通常作为各种运动平台的核心导航信息源。

目前，几乎所有类型惯性导航系统的解算算法都存在两大缺陷：1)原理性缺陷：惯导算法对理论方程采用了近似，如姿态算法使用了简化的旋转向量；速度算法使用了姿态的一阶近似等；2)算法性缺陷：惯导算法参数在特殊运动情形下设计，如圆锥或划摇运动，因此在实际运动中不能保证最优性。

针对姿态算法的以上问题，申请号为cn201710273489.3的发明专利中提出一种基于函数迭代积分的刚体姿态解算方法，即：根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；利用角速度的多项式拟合函数以及罗德里格向量(rodrigues)积分方程，迭代计算罗德里格向量，进而根据迭代结果，以四元数的形式给出时间区间上的姿态变化。该方法具有计算精度高的优势，但在迭代过程中没有充分利用切比雪夫多项式的良好性质，且罗德里格向量的多项式阶数随着迭代过程急剧增长，计算量大，难以满足实时应用。随后，申请号为cn201810236436.9的发明专利中提出了对应的基于函数迭代积分的刚体姿态解算快速计算方法及系统，该快速算法利用拟合角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断，大幅提升了计算效率。以上两项发明专利申请较圆满地解决了惯导解算算法中的高精度姿态解算问题。

速度和位置解算中存在的以上缺陷缺乏有效的解决方法。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于函数迭代积分的惯性导航解算方法及系统。

根据本发明提供的一种基于函数迭代积分的惯性导航解算方法，包括：

拟合步骤：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力的切比雪夫多项式函数；

姿态迭代解算步骤：利用得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

速度/位置迭代解算步骤：利用得到的比力的切比雪夫多项式系数、姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

获取姿态/速度/位置步骤：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。

较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值，所述加速度计测量值包括比力测量值或者速度增量测量值。

较佳的，所述拟合步骤中将一时间区间划分为至少两个小时间区间，并依次解算。

较佳的，在姿态迭代解算和速度/位置迭代解算中，迭代直至最大迭代次数或预定的收敛条件。

较佳的，还包括：

导航步骤：根据对应时间区间上的姿态/速度/位置信息进行导航。

根据本发明提供的一种基于函数迭代积分的惯性导航解算系统，包括：

拟合模块：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力的切比雪夫多项式函数；

姿态迭代解算模块：利用得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

速度/位置迭代解算模块：利用得到的比力的切比雪夫多项式系数、姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

获取姿态/速度/位置模块：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。

较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值，所述加速度计测量值包括比力测量值或者速度增量测量值。

较佳的，所述拟合模块中将一时间区间划分为至少两个小时间区间，并依次解算。

较佳的，在姿态迭代解算和速度/位置迭代解算中，迭代直至最大迭代次数或预定的收敛条件。

较佳的，还包括：

导航模块：根据对应时间区间上的姿态/速度/位置信息进行导航。

与现有技术相比，本发明具有如下的有益效果：

本方法基于函数迭代积分和切比雪夫多项式近似的技术，根据陀螺和加速度计测量信息拟合重建角速度和比力加速度，并利用函数迭代积分方法实现姿态、速度和位置运动学方程的精确积分计算。迭代计算过程采用具有良好数值特性的切比雪夫多项式，将姿态、速度和位置的迭代积分变换为对应的切比雪夫多项式系数的迭代计算，并运用阶数截断的方法在不显著降低计算精度的情况下提高计算速度。本发明的优点是有效消除了运动引起的不可交换性误差，如姿态圆锥误差、速度划摇误差和位置卷轴误差等。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的解算流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明提供的基于函数迭代积分的惯导解算方法可以选择不同的计算参考坐标系，同时考虑到计算量问题，本发明以地球坐标系作为计算参考坐标系进行示例说明。

如图1所示，本发明提供的一种基于函数迭代积分的惯性导航解算方法及系统，包括：拟合步骤：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力加速度的切比雪夫多项式函数。陀螺测量值包括角速度测量值或者角增量测量值，加速度计测量值包括比力(非万有引力加速度)测量值或者速度测量值。

姿态迭代解算步骤：利用拟合步骤得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。姿态迭代过程持续进行，直到达到最大迭代次数或事先指定的收敛条件。

速度/位置迭代解算步骤：利用拟合步骤得到的比力的切比雪夫多项式系数、姿态迭代解算步骤最终得到的姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。速度/位置迭代过程持续进行，直到达到最大迭代次数或事先指定的收敛条件。

获取姿态/速度/位置步骤：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。

第一类切比雪夫多项式在区间[-11]上定义，并由以下迭代关系给出：

f0(x)＝1，f1(x)＝x，fi+1(x)＝2xfi(x)-fi-1(x)

其中fi(x)为第i阶第一类切比雪夫多项式。

拟合步骤：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力加速度的切比雪夫多项式函数；

对于tk时刻的n个陀螺的角速度测量值或角增量测量值以及n个加速度计的比力测量值或速度增量测量值k＝1，2，...n。令τ为映射后的时间自变量，将原时间区间映射到[-11]上。角速度采用不超过n-1阶的切比雪夫多项式进行拟合近似

比力采用不超过n-1阶的切比雪夫多项式进行拟合近似

其中nω和nf分别为角速度切比雪夫多项式和比力切比雪夫多项式的阶数，ci和di分别为角速度和比力的第i阶切比雪夫多项式的系数向量，τ为映射后的时间自变量。

对于角速度/比力测量的情况，系数ci和di通过求解如下方程来确定：

而角增量/速度增量测量的情况，系数ci和di通过求解如下方程来确定：

其中，上标t表示向量或矩阵转置，矩阵γ和θ按如下定义：

而函数按如下定义：

姿态迭代解算步骤：利用拟合步骤得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。

假定在l次迭代时，姿态四元数的切比雪夫多项式记做

其中mq为事先确定的截断阶数，bl，i为l次迭代时第i阶切比雪夫多项式的系数。当l＝0时，q0(t)≡q(0)，其中q(0)为初始姿态四元数。姿态四元数的切比雪夫多项式系数可按如下迭代计算：

直到满足收敛条件或达到事先设定的最大迭代次数。收敛条件可以根据前后两次迭代的切比雪夫多项式系数间的欧拉距离偏差进行设置，即姿态迭代计算的最终结果记为用作后续速度和位置计算的输入。

速度/位置迭代解算步骤：利用拟合步骤得到的比力的切比雪夫多项式系数、姿态迭代解算步骤最终得到的姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。

根据比力切比雪夫多项式以及姿态四元数的切比雪夫多项式，比力变换的积分可写作

假定在l次迭代时，速度/位置的切比雪夫多项式记做

其中mv和mp为事先确定的速度和位置的截断阶数，sl，i和ρl,i为速度和位置在l次迭代时的第i阶切比雪夫多项式的系数。当l＝0时，其中v^e(0)和p^e(0)分别为初始速度和初始位置。

考虑到重力模型一般在地理坐标系中给出且重力是位置的非线性函数，利用切比雪夫多项式对地球坐标系下的重力向量进行如下近似

其中为地球坐标系相对于地理坐标系的姿态矩阵(也是位置的非线性函数)，mg为重力的切比雪夫多项式的最大阶数，ecef2lla(·)表示地球直角坐标到地理坐标(经度、纬度和高度)的变换。重力向量的切比雪夫多项式系数γl,i按如下计算

其中δ0i为克罗内克δ函数(当i＝1时输出1；当i为其它时输出0)。项数p越大，重力的近似越精确。

代入公式(9)和(12)，速度/位置的切比雪夫多项式系数可按如下迭代计算：

直到满足收敛条件或达到事先设定的最大迭代次数。

公式(13)中的速度和位置顺序可以对调，同一次迭代中先计算出来的速度(位置)结果可直接用于后续的位置(速度)计算，比如公式(13)可以按如下顺序计算

和

注意，公式(15)的重力向量近似采用了公式(14)计算出来的位置(对应于γl+1,i)而不是上一次迭代中的

获取姿态/速度/位置步骤：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式,计算得到对应时间区间上的姿态/速度/位置信息。

对于长时间区间上的惯导解算，可将其划分为若干个小时间区间，依次计算实现。

导航步骤：根据对应时间区间上的姿态/速度/位置信息进行导航。

根据上述一种基于函数迭代积分的惯性导航解算方法，本发明还提供一种基于函数迭代积分的惯性导航解算系统，包括：

拟合模块：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力的切比雪夫多项式函数；

姿态迭代解算模块：利用得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

速度/位置迭代解算模块：利用得到的比力的切比雪夫多项式系数、姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；

获取姿态/速度/位置模块：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。