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

权利要求书

1.一种基于函数迭代积分的刚体姿态解算方法，其特征在于，包括：

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

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

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

其中，所述拟合步骤具体包括：

对于t_k时刻的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；

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

迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n_T≥N。

2.根据权利要求1所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，所述陀螺测量值包括角速度测量值或者角增量测量值。

3.一种基于函数迭代积分的刚体姿态解算系统，其特征在于，包括：

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

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

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

其中，所述拟合模块具体包括：

对于t_k时刻的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；

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

迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n_T≥N。

4.根据权利要求3所述的基于函数迭代积分的刚体姿态解算系统，其特征在于，所述陀螺测量值包括角速度测量值或者角增量测量值。