[发明专利]基于函数迭代积分的刚体姿态解算方法及系统

说明书

本发明提供了一种基于函数迭代积分的刚体姿态解算方法及系统，包括：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。本发明基于函数迭代积分的技术，利用罗德里格向量，实现从陀螺测量快速重建姿态，在不显著降低计算精度的情况下提高计算速度。

技术领域

本发明涉及测试测量技术领域，具体而言，涉及一种基于函数迭代积分的刚体姿态解算快速计算方法及系统。

背景技术

三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。与速度、位置等平移运动不同，姿态不能被直接测量，只能通过角速度积分或向量匹配等间接方式获得。角速度积分方式的姿态解算是完全自主的，不需要外部信息辅助，因此在很多(如卫星导航系统不能发挥作用的)应用场合备受青睐。

近年来，本领域研究人员提出了若干高精度姿态解算方法。申请人在申请号为CN201710273489.3的发明专利中提出一种基于函数迭代积分的刚体姿态解算方法，即：根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；利用角速度的多项式拟合函数以及罗德里格向量(Rodrigues)积分方程，迭代计算罗德里格向量，进而根据迭代结果，以四元数的形式给出时间区间上的姿态变化。该方法具有计算精度高的优势，但在迭代过程中没有充分利用切比雪夫多项式的良好性质，且罗德里格向量的多项式阶数随着迭代过程急剧增长，计算量大，难以满足实时应用。例如，对于利用八个陀螺测量值进行角速度多项式拟合的情况，在第七次迭代时，罗德里格向量多项式的阶数超过一千！实际上，由于角速度测量存在误差，对罗德里格向量不需要用到如此高阶多项式。

发明内容

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

根据本发明提供的一种基于函数迭代积分的刚体姿态解算方法，包括：

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

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

姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。

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

较佳的，拟合步骤具体包括：

对于t_k时刻的N个角速度测量值或角增量测量值k＝1,2,...N；令将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似

其中n为角速度切比雪夫多项式的阶数，c_i为第i阶切比雪夫多项式的系数向量，F_i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。

较佳的，迭代步骤具体包括：

在l次迭代时，罗德里格向量的切比雪夫多项式记做：

其中n_T为预设的截断阶数，b_l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g_l＝0。

较佳的，罗德里格向量的切比雪夫多项式系数按如下迭代计算：