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

权利要求书

1.一种基于函数迭代积分的惯性导航解算方法，其特征在于，包括：

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

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

比力采用不超过N-1阶的切比雪夫多项式进行拟合近似

其中n_ω和n_f分别为角速度切比雪夫多项式和比力切比雪夫多项式的阶数，c_i和d_i分别为角速度和比力的第i阶切比雪夫多项式的系数向量，τ为映射后的时间自变量；

对于角速度/比力测量的情况，系数c_i和d_i通过求解如下方程来确定：

而角增量/速度增量测量的情况，系数c_i和d_i通过求解如下方程来确定：

其中，上标T表示向量或矩阵转置，矩阵Γ和Θ按如下定义：

而函数按如下定义：

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

假定在l次迭代时，姿态四元数的切比雪夫多项式记做

其中m_q为事先确定的截断阶数，b_l,i为l次迭代时第i阶切比雪夫多项式的系数；当l＝0时，q₀(t)≡q(0)，其中q(0)为初始姿态四元数；姿态四元数的切比雪夫多项式系数按如下迭代计算：

直到满足收敛条件或达到事先设定的最大迭代次数，收敛条件根据前后两次迭代的切比雪夫多项式系数间的欧拉距离偏差进行设置，即姿态迭代计算的最终结果记为用作后续速度和位置计算的输入；

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

根据比力切比雪夫多项式以及姿态四元数的切比雪夫多项式，比力变换的积分写作

假定在l次迭代时，速度/位置的切比雪夫多项式记做

其中m_v和m_p为事先确定的速度和位置的截断阶数，s_l,i和ρ_l,i为速度和位置在l次迭代时的第i阶切比雪夫多项式的系数，当l＝0时，其中v^e(0)和p^e(0)分别为初始速度和初始位置；

考虑到重力模型在地理坐标系中给出且重力是位置的非线性函数，利用切比雪夫多项式对地球坐标系下的重力向量进行如下近似

其中为地球坐标系相对于地理坐标系的姿态矩阵，m_g为重力的切比雪夫多项式的最大阶数，ecef2lla(·)表示地球直角坐标到地理坐标的变换；重力向量的切比雪夫多项式系数γ_l,i按如下计算

其中δ_0i为克罗内克δ函数，当i＝1时输出1；当i为其它时输出0，项数P越大，重力的近似越精确；

代入公式(9)和(12)，速度/位置的切比雪夫多项式系数按如下迭代计算：

直到满足收敛条件或达到事先设定的最大迭代次数；

公式(13)中的速度和位置顺序对调，同一次迭代中先计算出来的速度结果直接用于后续的位置计算，公式(13)按如下顺序计算

和

公式(15)的重力向量近似采用了公式(14)计算出来的位置而不是上一次迭代中的

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