本发明涉及测试测量技术领域，具体而言，涉及一种基于函数迭代积分的刚体姿态解算快速计算方法及系统。
背景技术：
三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。与速度、位置等平移运动不同，姿态不能被直接测量，只能通过角速度积分或向量匹配等间接方式获得。角速度积分方式的姿态解算是完全自主的，不需要外部信息辅助，因此在很多(如卫星导航系统不能发挥作用的)应用场合备受青睐。
近年来，本领域研究人员提出了若干高精度姿态解算方法。申请人在申请号为cn201710273489.3的发明专利中提出一种基于函数迭代积分的刚体姿态解算方法，即：根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；利用角速度的多项式拟合函数以及罗德里格向量(rodrigues)积分方程，迭代计算罗德里格向量，进而根据迭代结果，以四元数的形式给出时间区间上的姿态变化。该方法具有计算精度高的优势，但在迭代过程中没有充分利用切比雪夫多项式的良好性质，且罗德里格向量的多项式阶数随着迭代过程急剧增长，计算量大，难以满足实时应用。例如，对于利用八个陀螺测量值进行角速度多项式拟合的情况，在第七次迭代时，罗德里格向量多项式的阶数超过一千！实际上，由于角速度测量存在误差，对罗德里格向量不需要用到如此高阶多项式。
技术实现要素：
针对现有技术中的缺陷，本发明的目的是提供一种基于函数迭代积分的刚体姿态解算方法及系统。
根据本发明提供的一种基于函数迭代积分的刚体姿态解算方法，包括：
拟合步骤：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
迭代步骤：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值。
较佳的，拟合步骤具体包括：
对于tk时刻的n个角速度测量值
其中n为角速度切比雪夫多项式的阶数，ci为第i阶切比雪夫多项式的系数向量，fi(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
较佳的，迭代步骤具体包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
其中nt为预设的截断阶数，bl,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，gl＝0。
较佳的，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过n-1阶，设置截断阶数nt≥n。
根据本发明提供的一种基于函数迭代积分的刚体姿态解算系统，包括：
拟合模块：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
迭代模块：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
姿态解算模块：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值。
较佳的，拟合模块具体包括：
对于tk时刻的n个角速度测量值
其中n为角速度切比雪夫多项式的阶数，ci为第i阶切比雪夫多项式的系数向量，fi(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
较佳的，迭代模块具体包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
其中nt为预设的截断阶数，bl,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，gl＝0。
较佳的，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过n-1阶，设置截断阶数nt≥n。
与现有技术相比，本发明具有如下的有益效果：
本发明基于函数迭代积分的技术，利用罗德里格向量，实现从陀螺测量快速重建姿态。陀螺测量重建采用具有良好数值特性的切比雪夫多项式，将罗德里格向量的迭代积分变换为对应的切比雪夫多项式系数的迭代计算，并运用阶数截断的方法在不显著降低计算精度的情况下提高计算速度。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，本发明提供的一种基于函数迭代积分的刚体姿态解算方法，包括：
拟合步骤：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数。陀螺测量值包括角速度测量值或者角增量测量值。
迭代步骤：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。
姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
第一类切比雪夫多项式在区间[-11]上定义，并由以下迭代关系给出：
f0(x)＝1,f1(x)＝x,fi+1(x)＝2xfi(x)-fi-1(x)
其中fi(x)为第i阶第一类切比雪夫多项式。
步骤1)根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
对于tk时刻的n个角速度测量值
其中n为角速度切比雪夫多项式的阶数，ci为第i阶切比雪夫多项式的系数向量，fi(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
在角速度测量的情况下，系数ci通过求解如下方程来确定：
而在角增量测量的情况下，系数ci通过求解如下方程来确定：
其中，t表示向量或矩阵转置，函数
步骤2)利用角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照事先确定的截断阶数进行多项式截断；
假定在l次迭代时，罗德里格向量的切比雪夫多项式记做
其中nt为事先确定的截断阶数，bl,i为l次迭代时第i阶切比雪夫多项式的系数。当l＝0时，gl＝0。罗德里格向量的切比雪夫多项式系数可按如下迭代计算：
直到满足收敛条件或达到事先设定的最大迭代次数。上式中，×表示向量叉乘。根据(1)式，因角速度的多项式近似精度不超过n-1阶，可设置截断阶数nt≥n。
步骤3)根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，给出以时间区间开始时刻为参考的姿态四元数。
根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算，参考(5)式计算罗德里格向量，得到以时间区间开始时刻为参考的姿态四元数。
对于长时间区间上的姿态解算，可将其划分为若干个小时间区间，依次计算实现。
原则上，如果能接受一定程度的精度损失，本发明的姿态解算快速方法也适用于其他三维姿态参数，如旋转向量。此时，需要对步骤2)中的(6)式和步骤3)中的(7)式做相应的调整如下：
步骤2)旋转向量的切比雪夫多项式系数可按如下迭代计算：
步骤3)根据旋转向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到旋转向量，给出以时间区间开始时刻为参考的姿态四元数。
在上述一种基于函数迭代积分的刚体姿态解算方法的基础上，本发明还提供一种基于函数迭代积分的刚体姿态解算系统，包括：
拟合模块：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数。陀螺测量值包括角速度测量值或者角增量测量值。
迭代模块：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。
姿态解算模块：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
具体的，拟合模块对于tk时刻的n个角速度测量值
其中n为角速度切比雪夫多项式的阶数，ci为第i阶切比雪夫多项式的系数向量，fi(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
在角速度测量的情况下，系数ci通过求解如下方程来确定：
而在角增量测量的情况下，系数ci通过求解如下方程来确定：
其中，t表示向量或矩阵转置，函数
具体的，迭代模块中罗德里格向量的切比雪夫多项式计算包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
其中nt为预设的截断阶数，bl,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，gl＝0。
罗德里格向量的切比雪夫多项式系数按如下迭代计算：
直到满足收敛条件或达到事先设定的最大迭代次数。上式中，×表示向量叉乘。根据(1)式，因角速度的多项式近似精度不超过n-1阶，可设置截断阶数nt≥n。
姿态解算模块根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，给出以时间区间开始时刻为参考的姿态四元数。
根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算，参考(5)式计算罗德里格向量，得到以时间区间开始时刻为参考的姿态四元数。
对于长时间区间上的姿态解算，可将其划分为若干个小时间区间，依次计算实现。
原则上，如果能接受一定程度的精度损失，本发明的刚体姿态解算系统也适用于其他三维姿态参数，如旋转向量。此时，需要对(6)式和(7)式做相应的调整如下：
迭代模块中，旋转向量的切比雪夫多项式系数可按如下迭代计算：
姿态解算模块根据旋转向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到旋转向量，给出以时间区间开始时刻为参考的姿态四元数。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。