[发明专利]一种基于函数迭代积分的惯性导航解算方法及系统

说明书

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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