一种基于函数迭代积分的惯性导航解算方法及系统技术方案

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

An Inertial Navigation Solution Method and System Based on Functional Iterative Integral

The invention provides an inertial navigation solution method and system based on function iteration integral, which includes: fitting step; attitude iteration solution step; velocity/position iteration solution step; and attitude/velocity/position acquisition step. Based on the technique of function iteration integral and Chebyshev polynomial approximation, the angular velocity and specific force acceleration are reconstructed according to the measurement information of gyroscope and accelerometer, and the precise integration of attitude, velocity and position kinematics equation is realized by function iteration integral method. The iteration process uses Chebyshev polynomials with good numerical characteristics. The iteration integral of attitude, velocity and position is transformed into the iteration calculation of the corresponding Chebyshev polynomial coefficients. The order truncation method is used to improve the calculation speed without significantly reducing the calculation accuracy. The advantages of the present invention are that the non-commutative errors caused by motion, such as attitude coning errors, speed screw errors and position scroll errors, are effectively eliminated.

【技术实现步骤摘要】
一种基于函数迭代积分的惯性导航解算方法及系统
本专利技术涉及惯性导航、机器人等
，具体而言，涉及一种基于函数迭代积分的惯性导航解算方法及系统。
技术介绍
三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。惯性导航利用陀螺和加速度计测量角速度和加速度信息，积分获得姿态、速度和位置信息。惯性导航的优点是运动信息丰富，工作过程完全自主，不需要外部信息辅助，特别在卫星导航信号据止或被干扰欺骗等应用场合备受青睐，通常作为各种运动平台的核心导航信息源。目前，几乎所有类型惯性导航系统的解算算法都存在两大缺陷：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阶第一类切比雪夫多项式。拟合步骤：根据时间区间上的陀螺测量值和加速度计测本文档来自技高网...

【技术保护点】
1.一种基于函数迭代积分的惯性导航解算方法，其特征在于，包括：拟合步骤：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力的切比雪夫多项式函数；姿态迭代解算步骤：利用得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；速度/位置迭代解算步骤：利用得到的比力的切比雪夫多项式系数、姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；获取姿态/速度/位置步骤：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。

【技术特征摘要】
1.一种基于函数迭代积分的惯性导航解算方法，其特征在于，包括：拟合步骤：根据时间区间上的陀螺测量值和加速度计测量值，拟合出角速度和比力的切比雪夫多项式函数；姿态迭代解算步骤：利用得到的角速度的切比雪夫多项式系数以及姿态四元数积分方程，迭代计算姿态四元数的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；速度/位置迭代解算步骤：利用得到的比力的切比雪夫多项式系数、姿态四元数切比雪夫多项式系数以及速度/位置积分方程，迭代计算速度/位置的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；获取姿态/速度/位置步骤：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息。2.根据权利要求1所述的基于函数迭代积分的惯性导航解算方法，其特征在于，所述陀螺测量值包括角速度测量值或者角增量测量值，所述加速度计测量值包括比力测量值或者速度增量测量值。3.根据权利要求1所述的基于函数迭代积分的惯性导航解算方法，其特征在于，所述拟合步骤中将一时间区间划分为至少两个小时间区间，并依次解算。4.根据权利要求1所述的基于函数迭代积分的惯性导航解算方法，其特征在于，在姿态迭代解算和速度/位置迭代解算中，迭代直至最大迭代次数或预定的收敛条件。5.根据权利要求1所述的基于函数迭代积分的惯性导航解算方法，其特征在于，还包括：导航步骤：根据对应时间区间上的姿态/速度/位置信息进行导航。...

【专利技术属性】
技术研发人员：武元新，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：上海,31